Analysis Software
Documentation for sPHENIX simulation 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/TrackFitUtils.h>
7 #include <trackbase/TrkrCluster.h>
8 #include <trackbase_historic/ActsTransformations.h>
9 #include <trackbase/ClusterErrorPara.h>
10 
11 #include <Geant4/G4SystemOfUnits.hh>
12 
13 #include <TMatrixFfwd.h>
14 #include <TMatrixT.h>
15 #include <TMatrixTUtils.h>
16 //#define _DEBUG_
17 
18 #if defined(_DEBUG_)
19 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp
20 #else
21 #define LogDebug(exp) (void)0
22 #endif
23 
24 #define LogError(exp) if(Verbosity()>0) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp
25 #define LogWarning(exp) if(Verbosity()>0) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp
26 
27 using keylist = std::vector<TrkrDefs::cluskey>;
28 
29 // anonymous namespace for local functions
30 namespace
31 {
32  // square
33  template<class T> inline constexpr T square( const T& x ) { return x*x; }
34 }
35 
36 bool ALICEKF::checknan(double val, const std::string &name, int num) const
37 {
38  if(std::isnan(val))
39  {
40  if(Verbosity()>0) std::cout << "WARNING: " << name << " is NaN for seed " << num << ". Aborting this seed.\n";
41  }
42  return std::isnan(val);
43 }
44 
45 double ALICEKF::get_Bz(double x, double y, double z) const
46 {
47  if(_use_const_field) return _const_field;
48  double p[4] = {x*cm,y*cm,z*cm,0.*cm};
49  double bfield[3];
50  _B->GetFieldValue(p,bfield);
51  return bfield[2]/tesla;
52 }
53 
54 double ALICEKF::getClusterError(TrkrCluster* c, TrkrDefs::cluskey key, Acts::Vector3 global, int i, int j) const
55 {
56  if(_use_fixed_clus_error)
57  {
58  if(i==j) return _fixed_clus_error.at(i)*_fixed_clus_error.at(i);
59  else return 0.;
60  }
61  else
62  {
63  TMatrixF localErr(3,3);
64 
65  double clusRadius = sqrt(global[0]*global[0] + global[1]*global[1]);
66  auto para_errors = _ClusErrPara->get_clusterv5_modified_error(c,clusRadius,key);
67 
68  localErr[0][0] = 0.;
69  localErr[0][1] = 0.;
70  localErr[0][2] = 0.;
71  localErr[1][0] = 0.;
72  localErr[1][1] = para_errors.first;
73  localErr[1][2] = 0.;
74  localErr[2][0] = 0.;
75  localErr[2][1] = 0.;
76  localErr[2][2] = para_errors.second;
77 
78  float clusphi = atan2(global(1), global(0));
79  TMatrixF ROT(3,3);
80  ROT[0][0] = cos(clusphi);
81  ROT[0][1] = -sin(clusphi);
82  ROT[0][2] = 0.0;
83  ROT[1][0] = sin(clusphi);
84  ROT[1][1] = cos(clusphi);
85  ROT[1][2] = 0.0;
86  ROT[2][0] = 0.0;
87  ROT[2][1] = 0.0;
88  ROT[2][2] = 1.0;
89  TMatrixF ROT_T(3,3);
90  ROT_T.Transpose(ROT);
91 
92  TMatrixF err(3,3);
93  err = ROT * localErr * ROT_T;
94 
95  return err[i][j];
96  }
97 }
98 
99 TrackSeedAliceSeedMap ALICEKF::ALICEKalmanFilter(const std::vector<keylist>& trackSeedKeyLists,bool use_nhits_limit, const PositionMap& globalPositions, std::vector<float>& trackChi2) const
100 {
101 // TFile* f = new TFile("/sphenix/u/mjpeters/macros_hybrid/detectors/sPHENIX/pull.root", "RECREATE");
102 // TNtuple* ntp = new TNtuple("pull","pull","cx:cy:cz:xerr:yerr:zerr:tx:ty:tz:layer:xsize:ysize:phisize:phierr:zsize");
103  std::vector<TrackSeed_v1> seeds_vector;
104  std::vector<Eigen::Matrix<double,6,6>> alice_seeds_vector;
105  int nseeds = 0;
106 
107  if(Verbosity()>0) std::cout << "min clusters per track: " << _min_clusters_per_track << "\n";
108  for( auto trackKeyChain:trackSeedKeyLists )
109  {
110  if(trackKeyChain.size()<2) continue;
111  if(use_nhits_limit && trackKeyChain.size() < _min_clusters_per_track) continue;
112  if(TrkrDefs::getLayer(trackKeyChain.front())<TrkrDefs::getLayer(trackKeyChain.back())) std::reverse(trackKeyChain.begin(),trackKeyChain.end());
113  // get starting cluster from key
114  // Transform sPHENIX coordinates into ALICE-compatible coordinates
115  const auto& globalpos = globalPositions.at(trackKeyChain.at(0));
116  double x0 = globalpos(0);
117  double y0 = globalpos(1);
118  double z0 = globalpos(2);;
119  LogDebug("Initial (x,y,z): (" << x0 << "," << y0 << "," << z0 << ")" << std::endl);
120  // ALICE x coordinate = distance from beampipe
121  double alice_x0 = sqrt(x0*x0+y0*y0);
122  double alice_y0 = 0;
123  double alice_z0 = z0;
124  // Initialize track and linearisation
125  GPUTPCTrackParam trackSeed;
126  trackSeed.InitParam();
127  trackSeed.SetX(alice_x0);
128  trackSeed.SetY(alice_y0);
129  trackSeed.SetZ(alice_z0);
130  double x = x0;
131  double y = y0;
132  #if defined(_DEBUG_)
133  double z = z0;
134  double alice_x = sqrt(x0*x0+y0*y0);
135  #endif
136  double trackCartesian_x = 0.;
137  double trackCartesian_y = 0.;
138  double trackCartesian_z = 0.;
139  // Pre-set momentum-based parameters to improve numerical stability
140  const auto& secondpos = globalPositions.at(trackKeyChain.at(1));
141 
142  const double second_x = secondpos(0);
143  const double second_y = secondpos(1);
144  const double second_z = secondpos(2);
145  const double first_phi = atan2(y0,x0);
146  const double second_alice_x = second_x*std::cos(first_phi)+second_y*std::sin(first_phi);
147  const double delta_alice_x = second_alice_x - alice_x0;
148  //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));
149  const double second_alice_y = -second_x*std::sin(first_phi)+second_y*std::cos(first_phi);
150  const double init_SinPhi = second_alice_y / std::sqrt(square(delta_alice_x) + square(second_alice_y));
151  const double delta_z = second_z - z0;
152  const double init_DzDs = -delta_z / std::sqrt(square(delta_alice_x) + square(second_alice_y));
153  trackSeed.SetSinPhi(init_SinPhi);
154  LogDebug("Set initial SinPhi to " << init_SinPhi << std::endl);
155  trackSeed.SetDzDs(init_DzDs);
156  LogDebug("Set initial DzDs to " << init_DzDs << std::endl);
157 
158  // get initial pt estimate
159  std::vector<std::pair<double,double>> pts;
160  std::transform( trackKeyChain.begin(), trackKeyChain.end(), std::back_inserter( pts ), [&globalPositions]( const TrkrDefs::cluskey& key )
161  {
162  const auto& clpos = globalPositions.at(key);
163  return std::make_pair(clpos(0),clpos(1));
164  });
165 
166  const auto [R, x_center, y_center] = TrackFitUtils::circle_fit_by_taubin( pts );
167  if(Verbosity()>1) std::cout << "circle fit parameters: R=" << R << ", X0=" << x_center << ", Y0=" << y_center << std::endl;
168 
169  // check circle fit success
170  /* failed fit will result in infinite momentum for the track, which in turn will break the kalman filter */
171  if( std::isnan(R) ) continue;
172 
173  double init_QPt = 1./(0.3*R/100.*get_Bz(x0,y0,z0));
174  // determine charge
175  double phi_first = atan2(y0,x0);
176  if(Verbosity()>1) std::cout << "phi_first: " << phi_first << std::endl;
177  double phi_second = atan2(second_y,second_x);
178  if(Verbosity()>1) std::cout << "phi_second: " << phi_second << std::endl;
179  double dphi = phi_second - phi_first;
180  if(Verbosity()>1) std::cout << "dphi: " << dphi << std::endl;
181  if(dphi>M_PI) dphi = 2*M_PI - dphi;
182  if(dphi<-M_PI) dphi = 2*M_PI + dphi;
183  if(Verbosity()>1) std::cout << "corrected dphi: " << dphi << std::endl;
184  if(dphi<0) init_QPt = -1*init_QPt;
185  LogDebug("initial QPt: " << init_QPt << std::endl);
186  trackSeed.SetQPt(init_QPt);
187 
188  GPUTPCTrackLinearisation trackLine(trackSeed);
189  GPUTPCTrackParam::GPUTPCTrackFitParam fp;
190  trackSeed.CalculateFitParameters(fp);
191 
192  LogDebug(std::endl << std::endl << "------------------------" << std::endl << "seed size: " << trackKeyChain.size() << std::endl << std::endl << std::endl);
193  int cluster_ctr = 1;
194 // bool aborted = false;
195  // starting at second cluster, perform track propagation
196  std::vector<double> cx;
197  std::vector<double> cy;
198  std::vector<double> cz;
199  std::vector<double> tx;
200  std::vector<double> ty;
201  std::vector<double> tz;
202  std::vector<double> xerr;
203  std::vector<double> yerr;
204  std::vector<double> zerr;
205  std::vector<double> layer;
206  std::vector<double> xsize;
207  std::vector<double> ysize;
208  std::vector<double> phisize;
209  std::vector<double> phierr;
210  std::vector<double> zsize;
211  for(auto clusterkey = std::next(trackKeyChain.begin()); clusterkey != trackKeyChain.end(); ++clusterkey)
212  {
213  if(std::isnan(trackSeed.GetX()) ||
214  std::isnan(trackSeed.GetY()) ||
215  std::isnan(trackSeed.GetZ())) continue;
216  LogDebug("-------------------------------------------------------------" << std::endl);
217  LogDebug("cluster " << cluster_ctr << " -> " << cluster_ctr + 1 << std::endl);
218  LogDebug("this cluster (x,y,z) = (" << x << "," << y << "," << z << ")" << std::endl);
219  LogDebug("layer " << (int)TrkrDefs::getLayer(*clusterkey) << std::endl);
220  // get cluster from key
221  TrkrCluster* nextCluster = _cluster_map->findCluster(*clusterkey);
222  const auto& nextpos = globalPositions.at(*clusterkey);
223 
224  // find ALICE x-coordinate
225  double nextCluster_x = nextpos(0);
226  double nextCluster_xerr = sqrt(getClusterError(nextCluster,*clusterkey,nextpos,0,0));
227  double nextCluster_y = nextpos(1);
228  double nextCluster_yerr = sqrt(getClusterError(nextCluster,*clusterkey,nextpos,1,1));
229  double nextCluster_z = nextpos(2);
230  double nextCluster_zerr = sqrt(getClusterError(nextCluster,*clusterkey,nextpos,2,2));
231  // rotate track coordinates to match orientation of next cluster
232  double newPhi = atan2(nextCluster_y,nextCluster_x);
233  LogDebug("new phi = " << newPhi << std::endl);
234  double oldPhi = atan2(y,x);
235  LogDebug("old phi = " << oldPhi << std::endl);
236  double alpha = newPhi - oldPhi;
237  LogDebug("alpha = " << alpha << std::endl);
238  if(!trackSeed.Rotate(alpha/2.,trackLine,_max_sin_phi))
239  {
240  LogWarning("Rotate failed! Aborting for this seed...\n");
241 // aborted = true;
242  break;
243  }
244  double nextAlice_x = nextCluster_x*cos(newPhi)+nextCluster_y*sin(newPhi);
245  LogDebug("track coordinates (ALICE) after rotation: (" << trackSeed.GetX() << "," << trackSeed.GetY() << "," << trackSeed.GetZ() << ")" << std::endl);
246  LogDebug("Transporting from " << alice_x << " to " << nextAlice_x << "...");
247  double track_x = trackSeed.GetX()*cos(newPhi)-trackSeed.GetY()*sin(newPhi);
248  double track_y = trackSeed.GetX()*sin(newPhi)+trackSeed.GetY()*cos(newPhi);
249  double track_z = trackSeed.GetZ();
250  if(!trackSeed.TransportToXWithMaterial((nextAlice_x+trackSeed.GetX())/2.,trackLine,fp,_Bzconst*get_Bz(track_x,track_y,track_z),_max_sin_phi))
251  {
252  LogWarning("Transport failed! Aborting for this seed...\n");
253 // aborted = true;
254  break;
255  }
256  if(!trackSeed.Rotate(alpha/2.,trackLine,_max_sin_phi))
257  {
258  LogWarning("Rotate failed! Aborting for this seed...\n");
259 // aborted = true;
260  break;
261  }
262  if(!trackSeed.TransportToXWithMaterial(nextAlice_x,trackLine,fp,_Bzconst*get_Bz(track_x,track_y,track_z),_max_sin_phi))
263  {
264  LogWarning("Transport failed! Aborting for this seed...\n");
265 // aborted = true;
266  break;
267  }
268  // }
269  // convert ALICE coordinates to sPHENIX cartesian coordinates, for debugging
270 
271  double predicted_alice_x = trackSeed.GetX();
272  LogDebug("new track ALICE x = " << trackSeed.GetX() << std::endl);
273  double predicted_alice_y = trackSeed.GetY();
274  LogDebug("new track ALICE y = " << trackSeed.GetY() << std::endl);
275  double predicted_z = trackSeed.GetZ();
276  LogDebug("new track z = " << trackSeed.GetZ() << std::endl);
277  double cos_phi = x/sqrt(x*x+y*y);
278  LogDebug("cos_phi = " << cos_phi << std::endl);
279  double sin_phi = y/sqrt(x*x+y*y);
280  LogDebug("sin phi = " << sin_phi << std::endl);
281  trackCartesian_x = predicted_alice_x*cos_phi-predicted_alice_y*sin_phi;
282  trackCartesian_y = predicted_alice_x*sin_phi+predicted_alice_y*cos_phi;
283  trackCartesian_z = predicted_z;
284  LogDebug("Track transported to (x,y,z) = (" << trackCartesian_x << "," << trackCartesian_y << "," << trackCartesian_z << ")" << std::endl);
285  LogDebug("Track position ALICE Y error: " << sqrt(trackSeed.GetCov(0)) << std::endl);
286  LogDebug("Track position x error: " << sqrt(trackSeed.GetCov(0))*sin_phi << std::endl);
287  LogDebug("Track position y error: " << sqrt(trackSeed.GetCov(0))*cos_phi << std::endl);
288  LogDebug("Track position z error: " << sqrt(trackSeed.GetCov(5)) << std::endl);
289  LogDebug("Next cluster is at (x,y,z) = (" << nextCluster_x << "," << nextCluster_y << "," << nextCluster_z << ")" << std::endl);
290  LogDebug("Cluster errors: (" << nextCluster_xerr << ", " << nextCluster_yerr << ", " << nextCluster_zerr << ")" << std::endl);
291  LogDebug("track coordinates (ALICE) after rotation: (" << trackSeed.GetX() << "," << trackSeed.GetY() << "," << trackSeed.GetZ() << ")" << std::endl);
292  //double nextCluster_alice_y = (nextCluster_x/cos(newPhi) - nextCluster_y/sin(newPhi))/(tan(newPhi)+1./tan(newPhi));
293  //double nextCluster_alice_y = 0.;
294  double nextCluster_alice_y = -nextCluster_x*sin(newPhi)+nextCluster_y*cos(newPhi);
295  LogDebug("next cluster ALICE y = " << nextCluster_alice_y << std::endl);
296  double y2_error = getClusterError(nextCluster,*clusterkey,nextpos,0,0)*sin(newPhi)*sin(newPhi)+2*getClusterError(nextCluster,*clusterkey,nextpos,0,1)*cos(newPhi)*sin(newPhi)+getClusterError(nextCluster,*clusterkey,nextpos,1,1)*cos(newPhi)*cos(newPhi);
297  double z2_error = getClusterError(nextCluster,*clusterkey,nextpos,2,2);
298  LogDebug("track ALICE SinPhi = " << trackSeed.GetSinPhi() << std::endl);
299  LogDebug("track DzDs = " << trackSeed.GetDzDs() << std::endl);
300  LogDebug("chi2 = " << trackSeed.GetChi2() << std::endl);
301  LogDebug("NDF = " << trackSeed.GetNDF() << std::endl);
302  LogDebug("chi2 / NDF = " << trackSeed.GetChi2()/trackSeed.GetNDF() << std::endl);
303 
304  // Apply Kalman filter
305  if(!trackSeed.Filter(nextCluster_alice_y,nextCluster_z,y2_error,z2_error,_max_sin_phi))
306  {
307  LogError("Kalman filter failed for seed " << nseeds << "! Aborting for this seed..." << std::endl);
308 // aborted = true;
309  break;
310  }
311  #if defined(_DEBUG_)
312  double track_pt = 1./trackSeed.GetQPt();
313  double track_pY = track_pt*trackSeed.GetSinPhi();
314  double track_pX = sqrt(track_pt*track_pt-track_pY*track_pY);
315  double track_px = track_pX*cos(newPhi)-track_pY*sin(newPhi);
316  double track_py = track_pX*sin(newPhi)+track_pY*cos(newPhi);
317  double track_pz = -track_pt*trackSeed.GetDzDs();
318  double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
319  #endif
320  LogDebug("track pt = " << track_pt << " +- " << track_pterr << std::endl);
321  LogDebug("track ALICE p = (" << track_pX << ", " << track_pY << ", " << track_pz << ")" << std::endl);
322  LogDebug("track p = (" << track_px << ", " << track_py << ", " << track_pz << ")" << std::endl);
323  x = nextCluster_x;
324  y = nextCluster_y;
325  #if defined(_DEBUG_)
326  z = nextCluster_z;
327  alice_x = nextAlice_x;
328  #endif
329  ++cluster_ctr;
330 
331  //if(cluster_ctr>10)
332  {
333 
334  float nextclusrad = std::sqrt(nextCluster_x*nextCluster_x +
335  nextCluster_y*nextCluster_y);
336  float nextclusphierr = nextCluster->getRPhiError() / nextclusrad;;
337 
338  cx.push_back(nextCluster_x);
339  cy.push_back(nextCluster_y);
340  cz.push_back(nextCluster_z);
341  tx.push_back(trackCartesian_x);
342  ty.push_back(trackCartesian_y);
343  tz.push_back(trackCartesian_z);
344  xerr.push_back(nextCluster_xerr);
345  yerr.push_back(nextCluster_yerr);
346  zerr.push_back(nextCluster_zerr);
347  layer.push_back(TrkrDefs::getLayer(*clusterkey));
348  phierr.push_back(nextclusphierr);
349  }
350  }
351 
352  //if(Verbosity()>0) std::cout << "finished track\n";
353 
354  double track_phi = atan2(y,x);
355 
356  double track_pt = fabs(1./trackSeed.GetQPt());
357  #if defined(_DEBUG_)
358  double track_pY = track_pt*trackSeed.GetSinPhi();
359  double track_pX = sqrt(track_pt*track_pt-track_pY*track_pY);
360  double track_px = track_pX*cos(track_phi)-track_pY*sin(track_phi);
361  double track_py = track_pX*sin(track_phi)+track_pY*cos(track_phi);
362  double track_pz = track_pt*trackSeed.GetDzDs();
363  #endif
364  double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
365  // If Kalman filter doesn't do its job (happens often with short seeds), use the circle-fit estimate as the central value
366  if(trackKeyChain.size()<10) track_pt = fabs(1./init_QPt);
367  LogDebug("track pt = " << track_pt << " +- " << track_pterr << std::endl);
368  LogDebug("track ALICE p = (" << track_pX << ", " << track_pY << ", " << track_pz << ")" << std::endl);
369  LogDebug("track p = (" << track_px << ", " << track_py << ", " << track_pz << ")" << std::endl);
370 
371 /*
372  if(cluster_ctr!=1 && !trackSeed.CheckNumericalQuality())
373  {
374  std::cout << "ERROR: Track seed failed numerical quality check before conversion to sPHENIX coordinates! Skipping this one.\n";
375  aborted = true;
376  continue;
377  }
378 */
379  // pt:z:dz:phi:dphi:c:dc
380  // Fill NT with track parameters
381  // double StartEta = -log(tan(atan(z0/sqrt(x0*x0+y0*y0))));
382 // if(aborted) continue;
383 // double track_pt = fabs( 1./(trackSeed.GetQPt()));
384  if(checknan(track_pt,"pT",nseeds)) continue;
385 // double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
386  if(checknan(track_pterr,"pT err",nseeds)) continue;
387  LogDebug("Track pterr = " << track_pterr << std::endl);
388  double track_x = trackSeed.GetX()*cos(track_phi)-trackSeed.GetY()*sin(track_phi);
389  double track_y = trackSeed.GetX()*sin(track_phi)+trackSeed.GetY()*cos(track_phi);
390  double track_z = trackSeed.GetZ();
391  if(checknan(track_z,"z",nseeds)) continue;
392  double track_zerr = sqrt(trackSeed.GetErr2Z());
393  if(checknan(track_zerr,"zerr",nseeds)) continue;
394  auto lcluster = _cluster_map->findCluster(trackKeyChain.back());
395  const auto& lclusterglob = globalPositions.at(trackKeyChain.back());
396  const float lclusterrad = sqrt(lclusterglob(0)*lclusterglob(0) + lclusterglob(1)*lclusterglob(1));
397  double last_cluster_phierr = lcluster->getRPhiError() / lclusterrad;;
398 
399  // phi error assuming error in track radial coordinate is zero
400  double track_phierr = sqrt(pow(last_cluster_phierr,2)+(pow(trackSeed.GetX(),2)*trackSeed.GetErr2Y()) /
401  pow(pow(trackSeed.GetX(),2)+pow(trackSeed.GetY(),2),2));
402  if(checknan(track_phierr,"phierr",nseeds)) continue;
403  LogDebug("Track phi = " << atan2(track_py,track_px) << std::endl);
404  LogDebug("Track phierr = " << track_phierr << std::endl);
405  double track_curvature = trackSeed.GetKappa(_Bzconst*get_Bz(track_x,track_y,track_z));
406  if(checknan(track_curvature,"curvature",nseeds)) continue;
407  double track_curverr = sqrt(trackSeed.GetErr2QPt())*_Bzconst*get_Bz(track_x,track_y,track_z);
408  if(checknan(track_curverr,"curvature error",nseeds)) continue;
409  TrackSeed_v1 track;
410 // track.set_vertex_id(_vertex_ids[best_vtx]);
411  for (unsigned int j = 0; j < trackKeyChain.size(); ++j)
412  {
413  track.insert_cluster_key(trackKeyChain.at(j));
414  }
415 
416  int track_charge = 0;
417  if(trackSeed.GetQPt()<0) track_charge = -1 * _fieldDir;
418  else track_charge = 1 * _fieldDir;
419 
420  double s = sin(track_phi);
421  double c = cos(track_phi);
422  double p = trackSeed.GetSinPhi();
423 
425  //track.set_x(trackSeed.GetX()*c-trackSeed.GetY()*s);//_vertex_x[best_vtx]); //track.set_x(cl->getX());
426  //track.set_y(trackSeed.GetX()*s+trackSeed.GetY()*c);//_vertex_y[best_vtx]); //track.set_y(cl->getY());
427  //track.set_z(trackSeed.GetZ());//_vertex_z[best_vtx]); //track.set_z(cl->getZ());
428  //if(Verbosity()>0) std::cout << "x " << track.get_x() << "\n";
429  //if(Verbosity()>0) std::cout << "y " << track.get_y() << "\n";
430  //if(Verbosity()>0) std::cout << "z " << track.get_z() << "\n";
431  //if(checknan(p,"ALICE sinPhi",nseeds)) continue;
432  double d = trackSeed.GetDzDs();
433  if(checknan(d,"ALICE dz/ds",nseeds)) continue;
434 
436  //double pY = track_pt*p;
437  //double pX = sqrt(track_pt*track_pt-pY*pY);
440  track.set_qOverR(trackSeed.GetQPt()*(0.3*_const_field)/100.);
441  //track.set_px(pX*c-pY*s);
442  //track.set_py(pX*s+pY*c);
443  //track.set_pz(track_pt * trackSeed.GetDzDs());
444  const double* cov = trackSeed.GetCov();
445  bool cov_nan = false;
446  for(int i=0;i<15;i++)
447  {
448  if(checknan(cov[i],"covariance element "+std::to_string(i),nseeds)) cov_nan = true;
449  }
450  if(cov_nan) continue;
451  // make this into an actual Eigen matrix
452  Eigen::Matrix<double,5,5> ecov;
453  ecov(0,0)=cov[0];
454  ecov(0,1)=cov[1];
455  ecov(0,2)=cov[2];
456  ecov(0,3)=cov[3];
457  ecov(0,4)=cov[4];
458  ecov(1,1)=cov[5];
459  ecov(1,2)=cov[6];
460  ecov(1,3)=cov[7];
461  ecov(1,4)=cov[8];
462  ecov(2,2)=cov[9];
463  ecov(2,3)=cov[10];
464  ecov(2,4)=cov[11];
465  ecov(3,3)=cov[12];
466  ecov(3,4)=cov[13];
467  ecov(4,4)=cov[14];
468  // symmetrize
469  ecov(1,0)=ecov(0,1);
470  ecov(2,0)=ecov(0,2);
471  ecov(3,0)=ecov(0,3);
472  ecov(4,0)=ecov(0,4);
473  ecov(2,1)=ecov(1,2);
474  ecov(3,1)=ecov(1,3);
475  ecov(4,1)=ecov(1,4);
476  ecov(3,2)=ecov(2,3);
477  ecov(4,2)=ecov(2,4);
478  ecov(4,3)=ecov(3,4);
479  // make rotation matrix based on the following:
480  // x = X*cos(track_phi) - Y*sin(track_phi)
481  // y = X*sin(track_phi) + Y*cos(track_phi)
482  // z = Z
483  // pY = pt*sinphi
484  // pX = sqrt(pt**2 - pY**2)
485  // px = pX*cos(track_phi) - pY*sin(track_phi)
486  // py = pX*sin(track_phi) + pY*cos(track_phi)
487  // pz = pt*(dz/ds)
488  Eigen::Matrix<double,6,5> J;
489  J(0,0) = -s; // dx/dY
490  J(0,1) = 0.; // dx/dZ
491  J(0,2) = 0.; // dx/d(sinphi)
492  J(0,3) = 0.; // dx/d(dz/ds)
493  J(0,4) = 0.; // dx/d(Q/pt)
494 
495  J(1,0) = c; // dy/dY
496  J(1,1) = 0.; // dy/dZ
497  J(1,2) = 0.; // dy/d(sinphi)
498  J(1,3) = 0.; // dy/d(dz/ds)
499  J(1,4) = 0.; // dy/d(Q/pt)
500 
501  J(2,0) = 0.; // dz/dY
502  J(2,1) = 1.; // dz/dZ
503  J(2,2) = 0.; // dz/d(sinphi)
504  J(2,3) = 0.; // dz/d(dz/ds)
505  J(2,4) = 0.; // dz/d(Q/pt)
506 
507  J(3,0) = 0.; // dpx/dY
508  J(3,1) = 0.; // dpx/dZ
509  J(3,2) = -track_pt*(p*c/sqrt(1-p*p)+s); // dpx/d(sinphi)
510  J(3,3) = 0.; // dpx/d(dz/ds)
511  J(3,4) = track_pt*track_pt*track_charge*(p*s-c*sqrt(1-p*p)); // dpx/d(Q/pt)
512 
513  J(4,0) = 0.; // dpy/dY
514  J(4,1) = 0.; // dpy/dZ
515  J(4,2) = track_pt*(c-p*s/sqrt(1-p*p)); // dpy/d(sinphi)
516  J(4,3) = 0.; // dpy/d(dz/ds)
517  J(4,4) = -track_pt*track_pt*track_charge*(p*c+s*sqrt(1-p*p)); // dpy/d(Q/pt)
518 
519  J(5,0) = 0.; // dpz/dY
520  J(5,1) = 0.; // dpz/dZ
521  J(5,2) = 0.; // dpz/d(sinphi)
522  J(5,3) = track_pt; // dpz/d(dz/ds)
523  J(5,4) = -track_pt*track_pt*track_charge*d; // dpz/d(Q/pt)
524 /* bool cov_rot_nan = false;
525  for(int i=0;i<6;i++)
526  {
527  for(int j=0;j<5;j++)
528  {
529  if(checknan(J(i,j),"covariance rotator element ("+std::to_string(i)+","+std::to_string(j)+")",nseeds))
530  {
531  cov_rot_nan = true;
532  continue;
533  }
534  }
535  }
536  if(cov_rot_nan) continue;
537 */
538  // the heavy lifting happens here
539  Eigen::Matrix<double,6,6> scov = J*ecov*J.transpose();
540  if(!covIsPosDef(scov))
541  {
542  repairCovariance(scov);
543  }
544  /*
545  // Derived from:
546  // 1) Taking the Jacobian of the conversion from (Y,Z,SinPhi,DzDs,Q/Pt) to (x,y,z,px,py,pz)
547  // 2) Computing (Jacobian)*(ALICE covariance matrix)*(transpose of Jacobian)
548  track.set_error(0, 0, cov[0]*s*s);
549  track.set_error(0, 1, -cov[0]*c*s);
550  track.set_error(0, 2, -cov[1]*s);
551  track.set_error(0, 3, cov[2]*s*s/q-cov[4]*s*(-c/(q*q)+p*s/(q*q)));
552  track.set_error(0, 4, -cov[2]*c*s/q-cov[4]*s*(-c*p/(q*q)-s/(q*q)));
553  track.set_error(0, 5, cov[4]*d*s/(q*q)-cov[3]*s/q);
554  track.set_error(1, 1, cov[0]*c*c);
555  track.set_error(1, 2, cov[1]*c);
556  track.set_error(1, 3, -cov[2]*c*s/q+cov[4]*c*(-c/(q*q)+p*s/(q*q)));
557  track.set_error(1, 4, cov[2]*c*c/q+cov[4]*c*(-c*p/(q*q)-s/(q*q)));
558  track.set_error(1, 5, cov[4]*d*c/(q*q)+cov[3]*c/q);
559  track.set_error(2, 2, cov[5]);
560  track.set_error(2, 3, -cov[6]*s/q+cov[8]*(-c/(q*q)+p*s/(q*q)));
561  track.set_error(2, 4, cov[6]*c/q+cov[8]*(-c*p/(q*q)-s/(q*q)));
562  track.set_error(2, 5, -cov[8]*d/(q*q)+cov[7]/q);
563  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))));
564  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))));
565  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))));
566  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))));
567  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))));
568  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));
569  // symmetrize covariance
570  track.set_error(1, 0, track.get_error(0, 1));
571  track.set_error(2, 0, track.get_error(0, 2));
572  track.set_error(3, 0, track.get_error(0, 3));
573  track.set_error(4, 0, track.get_error(0, 4));
574  track.set_error(5, 0, track.get_error(0, 5));
575  track.set_error(2, 1, track.get_error(1, 2));
576  track.set_error(3, 1, track.get_error(1, 3));
577  track.set_error(4, 1, track.get_error(1, 4));
578  track.set_error(5, 1, track.get_error(1, 5));
579  track.set_error(3, 2, track.get_error(2, 3));
580  track.set_error(4, 2, track.get_error(2, 4));
581  track.set_error(5, 2, track.get_error(2, 5));
582  track.set_error(4, 3, track.get_error(3, 4));
583  track.set_error(5, 3, track.get_error(3, 5));
584  track.set_error(5, 4, track.get_error(4, 5));
585 */
586 
587 /*
588  for(int w=0;w<cx.size();w++)
589  {
590  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]);
591  }
592  cx.clear();
593  cy.clear();
594  cz.clear();
595  tx.clear();
596  ty.clear();
597  tz.clear();
598  xerr.clear();
599  yerr.clear();
600  zerr.clear();
601  layer.clear();
602  xsize.clear();
603  ysize.clear();
604  phisize.clear();
605  phierr.clear();
606  zsize.clear();
607 */
608  seeds_vector.push_back(track);
609  alice_seeds_vector.push_back(scov);
610  trackChi2.push_back(trackSeed.GetChi2() / trackSeed.GetNDF());
611 
612  ++nseeds;
613  }
614 // f->cd();
615 // ntp->Write();
616 // f->Close();
617  if(Verbosity()>0) std::cout << "number of seeds: " << nseeds << "\n";
618 
619  return std::make_pair(seeds_vector, alice_seeds_vector);
620 
621 }
622 
623 bool ALICEKF::covIsPosDef(Eigen::Matrix<double,6,6>& cov) const
624 {
625  // attempt Cholesky decomposition
626  Eigen::LLT<Eigen::Matrix<double,6,6>> chDec(cov);
627  // if Cholesky decomposition does not exist, matrix is not positive definite
628  return (chDec.info() != Eigen::NumericalIssue);
629 }
630 
631 void ALICEKF::repairCovariance(Eigen::Matrix<double,6,6>& cov) const
632 {
633  Eigen::Matrix<double,6,6> repaircov = cov;
634  // find closest positive definite matrix
635  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6>> solver(repaircov);
636  Eigen::Matrix<double,6,1> D = solver.eigenvalues();
637  Eigen::Matrix<double,6,6> Q = solver.eigenvectors();
638  Eigen::Matrix<double,6,1> Dp = D.cwiseMax(1e-15);
639  Eigen::Matrix<double,6,6> Z = Q*Dp.asDiagonal()*Q.transpose();
640  // updates covariance matrix
641  for(int i=0;i<6;i++)
642  {
643  for(int j=0;j<6;j++)
644  {
645  cov(i,j) = Z(i,j);
646  }
647  }
648 
649 }
650 
651 std::vector<double> ALICEKF::GetCircleClusterResiduals(const std::vector<std::pair<double,double>>& points, double R, double X0, double Y0) const
652 {
653  std::vector<double> residues;
654  std::transform( points.begin(), points.end(), std::back_inserter( residues ), [R,X0,Y0]( const std::pair<double,double>& point )
655  {
656  double x = point.first;
657  double y = point.second;
658 
659  // The shortest distance of a point from a circle is along the radial; line from the circle center to the point
660  return std::sqrt( square(x-X0) + square(y-Y0) ) - R;
661  } );
662  return residues;
663 }
664 
665 std::vector<double> ALICEKF::GetLineClusterResiduals(const std::vector<std::pair<double,double>>& points, double A, double B) const
666 {
667  std::vector<double> residues;
668  // calculate cluster residuals from the fitted circle
669  std::transform( points.begin(), points.end(), std::back_inserter( residues ), [A,B]( const std::pair<double,double>& point )
670  {
671  double r = point.first;
672  double z = point.second;
673 
674  // The shortest distance of a point from a circle is along the radial; line from the circle center to the point
675 
676  double a = -A;
677  double b = 1.0;
678  double c = -B;
679  return std::abs(a*r+b*z+c)/sqrt(square(a)+square(b));
680  });
681  return residues;
682 }