ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PHSimpleKFProp.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file PHSimpleKFProp.cc
1 
7 #include "PHSimpleKFProp.h"
8 
9 #include "ALICEKF.h"
10 #include "AssocInfoContainer.h"
11 #include "nanoflann.hpp"
12 #include "GPUTPCTrackParam.h"
14 
16 
19 
20 #include <phfield/PHField.h>
21 #include <phfield/PHFieldUtility.h>
22 
23 #include <phool/getClass.h>
24 #include <phool/phool.h> // for PHWHERE
25 
26 // tpc distortion correction
28 
32 
33 #include <trackbase/TrkrCluster.h>
36 #include <trackbase/TrkrDefs.h>
38 
39 #include <Geant4/G4SystemOfUnits.hh>
40 
41 #include <Eigen/Core>
42 #include <Eigen/Dense>
43 
44 #include <iostream> // for operator<<, basic_ostream
45 #include <vector>
46 
47 //#define _DEBUG_
48 
49 #if defined(_DEBUG_)
50 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp
51 #else
52 #define LogDebug(exp) (void)0
53 #endif
54 
55 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp
56 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp
57 
58 // anonymous namespace for local functions
59 namespace
60 {
61  // square
62  template<class T> inline constexpr T square( const T& x ) { return x*x; }
63 
64 // void line_fit(const std::vector<std::pair<double,double>>& points, double &a, double &b)
65 // {
66 // // copied from: https://www.bragitoff.com
67 // // we want to fit z vs radius
68 //
69 // double xsum=0,x2sum=0,ysum=0,xysum=0; //variables for sums/sigma of xi,yi,xi^2,xiyi etc
70 // for( const auto& point:points )
71 // {
72 // double r = point.first;
73 // double z = point.second;
74 //
75 // xsum=xsum+r; //calculate sigma(xi)
76 // ysum=ysum+z; //calculate sigma(yi)
77 // x2sum=x2sum+square(r); //calculate sigma(x^2i)
78 // xysum=xysum+r*z; //calculate sigma(xi*yi)
79 // }
80 //
81 // a=(points.size()*xysum-xsum*ysum)/(points.size()*x2sum-xsum*xsum); //calculate slope
82 // b=(x2sum*ysum-xsum*xysum)/(x2sum*points.size()-xsum*xsum); //calculate intercept
83 //
84 // return;
85 // }
86 
87 // void line_fit_clusters(const std::vector<TrkrCluster*>& clusters, double &a, double &b)
88 // {
89 // // convert to points
90 // std::vector<std::pair<double,double>> points;
91 // std::transform( clusters.begin(), clusters.end(), std::back_inserter( points ), []( TrkrCluster* cluster )
92 // {
93 // double x = cluster->getX();
94 // double y = cluster->getY();
95 // double r = sqrt(square(x)+square(y));
96 // double z = cluster->getZ();
97 // return std::make_pair( r, z );
98 // } );
99 //
100 // line_fit(points, a, b);
101 // }
102 
103  void CircleFitByTaubin ( const std::vector<Acts::Vector3F>& points, double &R, double &X0, double &Y0)
104  /*
105  Circle fit to a given set of data points (in 2D)
106  This is an algebraic fit, due to Taubin, based on the journal article
107  G. Taubin, "Estimation Of Planar Curves, Surfaces And Nonplanar
108  Space Curves Defined By Implicit Equations, With
109  Applications To Edge And Range Image Segmentation",
110  IEEE Trans. PAMI, Vol. 13, pages 1115-1138, (1991)
111  */
112  {
113  int iter,IterMAX=99;
114 
115  double Mz,Mxy,Mxx,Myy,Mxz,Myz,Mzz,Cov_xy,Var_z;
116  double A0,A1,A2,A22,A3,A33;
117  double x,y;
118  double DET,Xcenter,Ycenter;
119 
120  // Compute x- and y- sample means
121  double meanX = 0;
122  double meanY = 0;
123  double weight = 0;
124  for( const auto& point:points )
125  {
126  meanX += point(0);
127  meanY += point(1);
128  weight++;
129  }
130  meanX /= weight;
131  meanY /= weight;
132 
133  // computing moments
134 
135  Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;
136  for( const auto& point:points )
137  {
138  double Xi = point(0) - meanX; // centered x-coordinates
139  double Yi = point(1) - meanY; // centered y-coordinates
140  double Zi = Xi*Xi + Yi*Yi;
141 
142  Mxy += Xi*Yi;
143  Mxx += Xi*Xi;
144  Myy += Yi*Yi;
145  Mxz += Xi*Zi;
146  Myz += Yi*Zi;
147  Mzz += Zi*Zi;
148  }
149  Mxx /= weight;
150  Myy /= weight;
151  Mxy /= weight;
152  Mxz /= weight;
153  Myz /= weight;
154  Mzz /= weight;
155 
156  Mz = Mxx + Myy;
157  Cov_xy = Mxx*Myy - Mxy*Mxy;
158  Var_z = Mzz - Mz*Mz;
159  A3 = 4*Mz;
160  A2 = -3*Mz*Mz - Mzz;
161  A1 = Var_z*Mz + 4*Cov_xy*Mz - Mxz*Mxz - Myz*Myz;
162  A0 = Mxz*(Mxz*Myy - Myz*Mxy) + Myz*(Myz*Mxx - Mxz*Mxy) - Var_z*Cov_xy;
163  A22 = A2 + A2;
164  A33 = A3 + A3 + A3;
165 
166  // finding the root of the characteristic polynomial
167  // using Newton's method starting at x=0
168  // (it is guaranteed to converge to the right root)
169 
170  for (x=0.,y=A0,iter=0; iter<IterMAX; iter++) // usually, 4-6 iterations are enough
171  {
172  double Dy = A1 + x*(A22 + A33*x);
173  double xnew = x - y/Dy;
174  if ((xnew == x)||(!std::isfinite(xnew))) break;
175  double ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
176  if (fabs(ynew)>=fabs(y)) break;
177  x = xnew; y = ynew;
178  }
179 
180  // computing parameters of the fitting circle
181 
182  DET = x*x - x*Mz + Cov_xy;
183  Xcenter = (Mxz*(Myy - x) - Myz*Mxy)/DET/2;
184  Ycenter = (Myz*(Mxx - x) - Mxz*Mxy)/DET/2;
185 
186  // assembling the output
187 
188  X0 = Xcenter + meanX;
189  Y0 = Ycenter + meanY;
190  R = std::sqrt(square(Xcenter) + square(Ycenter));
191  }
192 
193 // void findRoot(const double R, const double X0, const double Y0, double& x, double& y)
194 // {
195 // /**
196 // * We need to determine the closest point on the circle to the origin
197 // * since we can't assume that the track originates from the origin
198 // * The eqn for the circle is (x-X0)^2+(y-Y0)^2=R^2 and we want to
199 // * minimize d = sqrt((0-x)^2+(0-y)^2), the distance between the
200 // * origin and some (currently, unknown) point on the circle x,y.
201 // *
202 // * Solving the circle eqn for x and substituting into d gives an eqn for
203 // * y. Taking the derivative and setting equal to 0 gives the following
204 // * two solutions. We take the smaller solution as the correct one, as
205 // * usually one solution is wildly incorrect (e.g. 1000 cm)
206 // */
207 //
208 // double miny = (sqrt(pow(X0, 2) * pow(R, 2) * pow(Y0, 2) + pow(R, 2)
209 // * pow(Y0,4)) + pow(X0,2) * Y0 + pow(Y0, 3))
210 // / (pow(X0, 2) + pow(Y0, 2));
211 //
212 // double miny2 = (-sqrt(pow(X0, 2) * pow(R, 2) * pow(Y0, 2) + pow(R, 2)
213 // * pow(Y0,4)) + pow(X0,2) * Y0 + pow(Y0, 3))
214 // / (pow(X0, 2) + pow(Y0, 2));
215 //
216 // double minx = std::sqrt(square(R) - square(miny - Y0)) + X0;
217 // double minx2 = -std::sqrt(square(R) - square(miny2 - Y0)) + X0;
218 //
219 // /// Figure out which of the two roots is actually closer to the origin
220 // if(fabs(minx) < fabs(minx2))
221 // x = minx;
222 // else
223 // x = minx2;
224 //
225 // if(fabs(miny) < fabs(miny2))
226 // y = miny;
227 // else
228 // y = miny2;
229 //
230 // }
231 
232 }
233 
234 using keylist = std::vector<TrkrDefs::cluskey>;
235 
237  : SubsysReco(name)
238 {}
239 
241 {
243 }
244 
246 {
247 
248  int ret = get_nodes(topNode);
249  if (ret != Fun4AllReturnCodes::EVENT_OK) return ret;
250 
251  _surfmaps = findNode::getClass<ActsSurfaceMaps>(topNode, "ActsSurfaceMaps");
252  if(!_surfmaps)
253  {
254  std::cout << "No Acts surface maps, exiting." << std::endl;
256  }
257 
258  _tgeometry = findNode::getClass<ActsTrackingGeometry>(topNode, "ActsTrackingGeometry");
259  if(!_tgeometry)
260  {
261  std::cout << "No Acts tracking geometry, exiting." << std::endl;
263  }
264 
265  // tpc distortion correction
266  m_dcc = findNode::getClass<TpcDistortionCorrectionContainer>(topNode,"TpcDistortionCorrectionContainer");
267  if( m_dcc )
268  { std::cout << "PHSimpleKFProp::InitRun - found TPC distortion correction container" << std::endl; }
269 
270  fitter = std::make_unique<ALICEKF>(topNode,_cluster_map,_fieldDir,
272  fitter->useConstBField(_use_const_field);
273  fitter->useFixedClusterError(_use_fixed_clus_err);
274  fitter->setFixedClusterError(0,_fixed_clus_err.at(0));
275  fitter->setFixedClusterError(1,_fixed_clus_err.at(1));
276  fitter->setFixedClusterError(2,_fixed_clus_err.at(2));
277  _field_map = PHFieldUtility::GetFieldMapNode(nullptr,topNode);
279 }
280 
281 double PHSimpleKFProp::get_Bz(double x, double y, double z) const
282 {
283  if(_use_const_field) return 1.4;
284  double p[4] = {x*cm,y*cm,z*cm,0.*cm};
285  double bfield[3];
286  _field_map->GetFieldValue(p,bfield);
287  return bfield[2]/tesla;
288 }
289 
291 {
292  //---------------------------------
293  // Get Objects off of the Node Tree
294  //---------------------------------
295 
297  _cluster_map = findNode::getClass<TrkrClusterContainer>(topNode, "TRKR_CLUSTER_TRUTH");
298  else
299  _cluster_map = findNode::getClass<TrkrClusterContainer>(topNode, "TRKR_CLUSTER");
300 
301  if (!_cluster_map)
302  {
303  std::cerr << PHWHERE << " ERROR: Can't find node TRKR_CLUSTER" << std::endl;
305  }
306 
307  _track_map = findNode::getClass<SvtxTrackMap>(topNode, "SvtxTrackMap");
308  if (!_track_map)
309  {
310  std::cerr << PHWHERE << " ERROR: Can't find SvtxTrackMap " << std::endl;
312  }
313 
314  _hitsets = findNode::getClass<TrkrHitSetContainer>(topNode, "TRKR_HITSET");
315  if(!_hitsets)
316  {
317  std::cerr << PHWHERE << "No hitset container on node tree. Bailing."
318  << std::endl;
320  }
321 
322  PHG4CylinderCellGeomContainer *geom_container =
323  findNode::getClass<PHG4CylinderCellGeomContainer>(topNode, "CYLINDERCELLGEOM_SVTX");
324  if (!geom_container)
325  {
326  std::cerr << PHWHERE << "ERROR: Can't find node CYLINDERCELLGEOM_SVTX" << std::endl;
328  }
329 
330  for(int i=7;i<=54;i++)
331  {
332  radii.push_back(geom_container->GetLayerCellGeom(i)->get_radius());
333  }
334 
336 }
337 
339 {
340  if(_n_iteration!=0){
341  _iteration_map = findNode::getClass<TrkrClusterIterationMapv1>(topNode, "CLUSTER_ITERATION_MAP");
342  if (!_iteration_map){
343  std::cerr << PHWHERE << "Cluster Iteration Map missing, aborting." << std::endl;
345  }
346  }
347 
348  if(Verbosity()>0) std::cout << "starting Process" << std::endl;
349  const auto globalPositions = PrepareKDTrees();
350  if(Verbosity()>0) std::cout << "prepared KD trees" << std::endl;
351  MoveToFirstTPCCluster(globalPositions);
352  if(Verbosity()>0) std::cout << "moved tracks into TPC" << std::endl;
353  std::vector<std::vector<TrkrDefs::cluskey>> new_chains;
354  std::vector<SvtxTrack> unused_tracks;
355  for(SvtxTrackMap::Iter track_it = _track_map->begin(); track_it != _track_map->end(); ++track_it )
356  {
357  // if not a TPC track, ignore
358  SvtxTrack* track = track_it->second;
359  const bool is_tpc = std::any_of(
360  track->begin_cluster_keys(),
361  track->end_cluster_keys(),
362  []( const TrkrDefs::cluskey& key ) { return TrkrDefs::getTrkrId(key) == TrkrDefs::tpcId; } );
363 
364  if(is_tpc)
365  {
366  if(Verbosity()>0) std::cout << "is tpc track" << std::endl;
367  new_chains.push_back(PropagateTrack(track, globalPositions));
368  }
369  else
370  {
371  // this is bad: it copies the track to its base class, which is essentially nothing
372  if(Verbosity()>0) std::cout << "is NOT tpc track" << std::endl;
373  unused_tracks.push_back(*track);
374  }
375  }
376 
377  _track_map->Reset();
378  std::vector<std::vector<TrkrDefs::cluskey>> clean_chains = RemoveBadClusters(new_chains, globalPositions);
379  std::vector<SvtxTrack_v2> ptracks = fitter->ALICEKalmanFilter(clean_chains,true, globalPositions);
380  publishSeeds(ptracks);
381  publishSeeds(unused_tracks);
383 }
384 
386 {
387  // get global position from Acts transform
388  auto globalpos = m_transform.getGlobalPosition(cluster,
389  _surfmaps,
390  _tgeometry);
391 
392  // check if TPC distortion correction are in place and apply
393  if( m_dcc ) { globalpos = m_distortionCorrection.get_corrected_position( globalpos, m_dcc ); }
394 
395  return globalpos;
396 }
397 
399 {
400  PositionMap globalPositions;
401  //***** convert clusters to kdhits, and divide by layer
402  std::vector<std::vector<std::vector<double> > > kdhits;
403  kdhits.resize(58);
404  if (!_cluster_map)
405  {
406  std::cout << "WARNING: (tracking.PHTpcTrackerUtil.convert_clusters_to_hits) cluster map is not provided" << std::endl;
407  return globalPositions;
408  }
409 
410  auto hitsetrange = _hitsets->getHitSets(TrkrDefs::TrkrId::tpcId);
411  for (auto hitsetitr = hitsetrange.first; hitsetitr != hitsetrange.second; ++hitsetitr)
412  {
413  auto range = _cluster_map->getClusters(hitsetitr->first);
414  for (TrkrClusterContainer::ConstIterator it = range.first; it != range.second; ++it)
415  {
416  TrkrDefs::cluskey cluskey = it->first;
417  TrkrCluster* cluster = it->second;
418  if(!cluster) continue;
419  if(_n_iteration!=0){
420  if(_iteration_map != NULL ){
421  // std::cout << "map exists entries: " << _iteration_map->size() << std::endl;
422  if(_iteration_map->getIteration(cluskey)>0){
423  //std::cout << "hit used, continue" << std::endl;
424  continue; // skip hits used in a previous iteration
425  }
426  }
427  }
428 
429  const Acts::Vector3D globalpos_d = getGlobalPosition(cluster);
430  const Acts::Vector3F globalpos = { (float) globalpos_d.x(), (float) globalpos_d.y(), (float) globalpos_d.z()};
431  globalPositions.insert(std::make_pair(cluskey, globalpos));
432 
433  int layer = TrkrDefs::getLayer(cluskey);
434  std::vector<double> kdhit(4);
435  kdhit[0] = globalpos_d.x();
436  kdhit[1] = globalpos_d.y();
437  kdhit[2] = globalpos_d.z();
438  uint64_t key = cluster->getClusKey();
439  std::memcpy(&kdhit[3], &key, sizeof(key));
440 
441  // HINT: way to get original uint64_t value from double:
442  //
443  // LOG_DEBUG("tracking.PHTpcTrackerUtil.convert_clusters_to_hits")
444  // << "orig: " << cluster->getClusKey() << ", readback: " << (*((int64_t*)&kdhit[3]));
445 
446  kdhits[layer].push_back(kdhit);
447  }
448  }
449  _ptclouds.resize(kdhits.size());
450  _kdtrees.resize(kdhits.size());
451  for(size_t l=0;l<kdhits.size();++l)
452  {
453  if(Verbosity()>0) std::cout << "l: " << l << std::endl;
454  _ptclouds[l] = std::make_shared<KDPointCloud<double>>();
455  _ptclouds[l]->pts.resize(kdhits[l].size());
456  if(Verbosity()>0) std::cout << "resized to " << kdhits[l].size() << std::endl;
457  for(size_t i=0;i<kdhits[l].size();++i)
458  {
459  _ptclouds[l]->pts[i] = kdhits[l][i];
460  }
461  _kdtrees[l] = std::make_shared<nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, KDPointCloud<double>>, KDPointCloud<double>, 3>>(3,*(_ptclouds[l]),nanoflann::KDTreeSingleIndexAdaptorParams(10));
462  _kdtrees[l]->buildIndex();
463  }
464 
465  return globalPositions;
466 }
467 
469 {
470  for(const auto& [key, track] : *_track_map)
471  {
472  double track_x = track->get_x();
473  double track_y = track->get_y();
474  if(sqrt(track_x*track_x+track_y*track_y)>10.)
475  {
476  if(Verbosity()>0) std::cout << "WARNING: attempting to move track to TPC which is already in TPC! Aborting for this track." << std::endl;
477  continue;
478  }
479 
480  // get cluster keys
481  std::vector<TrkrDefs::cluskey> ckeys;
482  std::copy(track->begin_cluster_keys(),track->end_cluster_keys(),std::back_inserter(ckeys));
483 
484  std::vector<Acts::Vector3F> trkGlobPos;
485  for(const auto& ckey : ckeys)
486  {
488  { trkGlobPos.push_back(globalPositions.at(ckey)); }
489  }
490 
491  // get circle fit for TPC clusters plus vertex
492  double R = 0;
493  double xc = 0;
494  double yc = 0;
495 
496  CircleFitByTaubin(trkGlobPos,R,xc,yc);
497  // want angle of tangent to circle at innermost (i.e. last) cluster
498  size_t inner_index;
499  if(TrkrDefs::getLayer(ckeys[0])>TrkrDefs::getLayer(ckeys.back()))
500  {
501  inner_index = ckeys.size()-1;
502  }
503  else
504  {
505  inner_index = 0;
506  }
507  double cluster_x = trkGlobPos.at(inner_index)(0);
508  double cluster_y = trkGlobPos.at(inner_index)(1);
509  double dy = cluster_y-yc;
510  double dx = cluster_x-xc;
511  double phi = atan2(dy,dx);
512  double dx0 = trkGlobPos.at(0)(0) - xc;
513  double dy0 = trkGlobPos.at(0)(1) - yc;
514  double phi0 = atan2(dy0, dx0);
515  double dx1 = trkGlobPos.at(1)(0) - xc;
516  double dy1 = trkGlobPos.at(1)(1) - yc;
517  double phi1 = atan2(dy1, dx1);
518  double dphi = phi1 - phi0;
519  if(dphi < 0)
520  phi += M_PI / 2.0;
521  else
522  phi -= M_PI / 2.0;
523  // rotate track momentum vector (pz stays the same)
524  double pt = track->get_pt();
525  track->set_px(pt*cos(phi));
526  track->set_py(pt*sin(phi));
527  // set track position
528  track->set_x(trkGlobPos.at(0)(0));
529  track->set_y(trkGlobPos.at(0)(1));
530  track->set_z(trkGlobPos.at(0)(2));
531  }
532 
533 }
534 
535 std::vector<TrkrDefs::cluskey> PHSimpleKFProp::PropagateTrack(SvtxTrack* track, const PositionMap& globalPositions) const
536 {
537  // extract cluster list
538  std::vector<TrkrDefs::cluskey> ckeys;
539  std::copy(track->begin_cluster_keys(),track->end_cluster_keys(),std::back_inserter(ckeys));
540  if(ckeys.size()>1 && ((int)TrkrDefs::getLayer(ckeys.front()))>((int)TrkrDefs::getLayer(ckeys.back())))
541  {
542  std::reverse(ckeys.begin(),ckeys.end());
543  }
544  // get track parameters
545  GPUTPCTrackParam kftrack;
546  kftrack.InitParam();
547  float track_phi = atan2(track->get_py(),track->get_px());
548  kftrack.SetQPt(track->get_charge()/track->get_pt());
549  float track_pX = track->get_px()*cos(track_phi)+track->get_py()*sin(track_phi);
550  float track_pY = -track->get_px()*sin(track_phi)+track->get_py()*cos(track_phi);
551  kftrack.SetSignCosPhi(track_pX/track->get_pt());
552  kftrack.SetSinPhi(track_pY/track->get_pt());
553  kftrack.SetDzDs(-track->get_pz()/track->get_pt());
554  Eigen::Matrix<double,6,6> xyzCov;
555  for(int i=0;i<6;i++)
556  {
557  for(int j=0;j<6;j++)
558  {
559  xyzCov(i,j) = track->get_error(i,j);
560  }
561  }
562  // Y = y
563  // Z = z
564  // SinPhi = py/sqrt(px^2+py^2)
565  // DzDs = pz/sqrt(px^2+py^2)
566  // QPt = 1/sqrt(px^2+py^2)
567 
568  const double track_px = track->get_px();
569  const double track_py = track->get_py();
570  const double track_pz = track->get_pz();
571  const double track_pt = std::sqrt( square( track_py ) + square( track_px ) );
572  const double track_pt3 = std::pow( track_pt, 3. );
573 
574  Eigen::Matrix<double,6,5> Jrot;
575  Jrot(0,0) = 0; // dY/dx
576  Jrot(1,0) = 1; // dY/dy
577  Jrot(2,0) = 0; // dY/dz
578  Jrot(3,0) = 0; // dY/dpx
579  Jrot(4,0) = 0; // dY/dpy
580  Jrot(5,0) = 0; // dY/dpz
581 
582  Jrot(0,1) = 0; // dZ/dx
583  Jrot(1,1) = 0; // dZ/dy
584  Jrot(2,1) = 1; // dZ/dz
585  Jrot(3,1) = 0; // dZ/dpx
586  Jrot(4,1) = 0; // dZ/dpy
587  Jrot(5,1) = 0; // dZ/dpz
588 
589  Jrot(0,2) = 0; // dSinPhi/dx
590  Jrot(1,2) = 0; // dSinPhi/dy
591  Jrot(2,2) = 0; // dSinPhi/dz
592  Jrot(3,2) = -track_py*track_px/track_pt3; // dSinPhi/dpx
593  Jrot(4,2) = track_px*track_px/track_pt3; // dSinPhi/dpy
594  Jrot(5,2) = 0; // dSinPhi/dpz
595 
596  Jrot(0,3) = 0; // dDzDs/dx
597  Jrot(1,3) = 0; // dDzDs/dy
598  Jrot(2,3) = 0; // dDzDs/dz
599  Jrot(3,3) = -track_px*track_pz/track_pt3; // dDzDs/dpx
600  Jrot(4,3) = -track_py*track_pz/track_pt3; // dDzDs/dpy
601  Jrot(5,3) = 1./track_pt; // dDzDs/dpz
602 
603  Jrot(0,4) = 0; // dQPt/dx
604  Jrot(1,4) = 0; // dQPt/dy
605  Jrot(2,4) = 0; // dQPt/dz
606  Jrot(3,4) = -track_px/track_pt3; // dQPt/dpx
607  Jrot(4,4) = -track_py/track_pt3; // dQPt/dpy
608  Jrot(5,4) = 0; // dQPt/dpz
609 
610  Eigen::Matrix<double,5,5> kfCov = Jrot.transpose()*xyzCov*Jrot;
611 
612  int ctr = 0;
613  for(int i=0;i<5;i++)
614  {
615  for(int j=0;j<5;j++)
616  {
617  if(i>=j)
618  {
619  kftrack.SetCov(ctr,kfCov(i,j));
620  ctr++;
621  }
622  }
623  }
624 
625  std::vector<TrkrDefs::cluskey> propagated_track;
626 
627  // setup ALICE track model on first cluster
628 // TrkrCluster* firstclus = _cluster_map->findCluster(ckeys[0]);
629 // TrkrCluster* lastclus = _cluster_map->findCluster(ckeys.back());
630 // double fx = firstclus->getX();
631 // double fy = firstclus->getY();
632 // double fz = firstclus->getZ();
633 // double fphi = atan2(fy,fx);
634 // double lx = lastclus->getX();
635 // double ly = lastclus->getY();
636 // double lz = lastclus->getZ();
637 // double lphi = atan2(ly,lx);
638 // kftrack.Rotate(fphi,kfline,10.);
639  kftrack.SetX(track->get_x()*cos(track_phi)+track->get_y()*sin(track_phi));
640  kftrack.SetY(-track->get_x()*sin(track_phi)+track->get_y()*cos(track_phi));
641  kftrack.SetZ(track->get_z());
642  if(Verbosity()>0)
643  {
644  std::cout << "initial track params:" << std::endl;
645  std::cout << "X: " << kftrack.GetX() << std::endl;
646  std::cout << "Y: " << kftrack.GetY() << std::endl;
647  std::cout << "Z: " << kftrack.GetZ() << std::endl;
648  std::cout << "SinPhi: " << kftrack.GetSinPhi() << std::endl;
649  std::cout << "DzDs: " << kftrack.GetDzDs() << std::endl;
650  std::cout << "QPt: " << kftrack.GetQPt() << std::endl;
651  }
652 // kftrack.Rotate(fphi,kfline,10.);
653 // double fX = fx*cos(fphi)+fy*sin(fphi);
654 // double oldphi = track_phi;
655 // double track_x = kftrack.GetX()*cos(oldphi)-kftrack.GetY()*sin(oldphi);
656 // double track_y = kftrack.GetX()*sin(oldphi)+kftrack.GetX()*cos(oldphi);
657 // kftrack.TransportToX(fX,kfline,_Bzconst*get_Bz(track_x,track_y,kftrack.GetZ()),10.);
658 // double new_phi = atan2(track_y,track_x);
659 // kftrack.Rotate(new_phi-oldphi,kfline,10.);
660 // kftrack.SetX(fx*cos(fphi)+fy*sin(fphi));
661 // kftrack.SetY(-fx*sin(fphi)+fy*cos(fphi));
662 // kftrack.SetZ(fz);
663  GPUTPCTrackLinearisation kfline(kftrack);
664 
665  // get layer for each cluster
666  std::vector<unsigned int> layers;
667  if(Verbosity()>0) std::cout << "cluster layers:" << std::endl;
668  std::transform( ckeys.begin(), ckeys.end(), std::back_inserter( layers ), []( const TrkrDefs::cluskey& key ) { return TrkrDefs::getLayer(key); } );
669 
670  double old_phi = track_phi;
671  unsigned int old_layer = TrkrDefs::getLayer(ckeys[0]);
672  if(Verbosity()>0) std::cout << "first layer: " << old_layer << std::endl;
673 
674  propagated_track.push_back(ckeys[0]);
675  // first, propagate downward
676  for(unsigned int l=old_layer+1;l<=54;l++)
677  {
678  if(std::isnan(kftrack.GetX()) ||
679  std::isnan(kftrack.GetY()) ||
680  std::isnan(kftrack.GetZ())) continue;
681  if(fabs(kftrack.GetZ())>105.) continue;
682  if(Verbosity()>0) std::cout << "\nlayer " << l << ":" << std::endl;
683  // check to see whether layer is already occupied by at least one cluster
684  // choosing the last one first (clusters organized from inside out)
685  bool layer_filled = false;
686  TrkrDefs::cluskey next_ckey = 0;
687  for(int k=layers.size()-1; k>=0; k--)
688  {
689  if(layer_filled) continue;
690  if(layers[k]==l)
691  {
692  layer_filled = true;
693  next_ckey = ckeys[k];
694  }
695  }
696  // if layer is already occupied, reset track parameters to last cluster in layer
697  if(layer_filled)
698  {
699  if(Verbosity()>0) std::cout << "layer is filled" << std::endl;
700  TrkrCluster* nc = _cluster_map->findCluster(next_ckey);
701  auto globalpos = globalPositions.at(next_ckey);
702  double cx = globalpos(0);
703  double cy = globalpos(1);
704  double cz = globalpos(2);
705  double cphi = atan2(cy,cx);
706  double cxerr = sqrt(fitter->getClusterError(nc,globalpos,0,0));
707  double cyerr = sqrt(fitter->getClusterError(nc,globalpos,1,1));
708  double czerr = sqrt(fitter->getClusterError(nc,globalpos,2,2));
709  double alpha = cphi-old_phi;
710  double tX = kftrack.GetX();
711  double tY = kftrack.GetY();
712  double tx = tX*cos(old_phi)-tY*sin(old_phi);
713  double ty = tX*sin(old_phi)+tY*cos(old_phi);
714  double tz = kftrack.GetZ();
715 // GPUTPCTrackParam::GPUTPCTrackFitParam fp;
716 // kftrack.CalculateFitParameters(fp);
717  if(Verbosity()>0) std::cout << "track position: (" << tx << ", " << ty << ", " << tz << ")" << std::endl;
718  kftrack.Rotate(alpha,kfline,10.);
719  kftrack.TransportToX(cx*cos(cphi)+cy*sin(cphi),kfline,_Bzconst*get_Bz(tx,ty,tz),10.);
720  if(std::isnan(kftrack.GetX()) ||
721  std::isnan(kftrack.GetY()) ||
722  std::isnan(kftrack.GetZ())) continue;
723  tX = kftrack.GetX();
724  tY = kftrack.GetY();
725  tx = tX*cos(cphi)-tY*sin(cphi);
726  ty = tX*sin(cphi)+tY*cos(cphi);
727  tz = kftrack.GetZ();
728  double tYerr = sqrt(kftrack.GetCov(0));
729  double tzerr = sqrt(kftrack.GetCov(5));
730  double txerr = fabs(tYerr*sin(cphi));
731  double tyerr = fabs(tYerr*cos(cphi));
732  if(Verbosity()>0) std::cout << "cluster position: (" << cx << ", " << cy << ", " << cz << ")" << std::endl;
733  if(Verbosity()>0) std::cout << "cluster position errors: (" << cxerr << ", " << cyerr << ", " << czerr << ")" << std::endl;
734  if(Verbosity()>0) std::cout << "new track position: (" << kftrack.GetX()*cos(cphi)-kftrack.GetY()*sin(cphi) << ", " << kftrack.GetX()*sin(cphi)+kftrack.GetY()*cos(cphi) << ", " << kftrack.GetZ() << ")" << std::endl;
735  if(Verbosity()>0) std::cout << "track position errors: (" << txerr << ", " << tyerr << ", " << tzerr << ")" << std::endl;
736  if(Verbosity()>0) std::cout << "distance: " << sqrt(square(kftrack.GetX()*cos(cphi)-kftrack.GetY()*sin(cphi)-cx)+square(kftrack.GetX()*sin(cphi)+kftrack.GetY()*cos(cphi)-cy)+square(kftrack.GetZ()-cz)) << std::endl;
737  if(fabs(tx-cx)<_max_dist*sqrt(txerr*txerr+cxerr*cxerr) &&
738  fabs(ty-cy)<_max_dist*sqrt(tyerr*tyerr+cyerr*cyerr) &&
739  fabs(tz-cz)<_max_dist*sqrt(tzerr*tzerr+czerr*czerr))
740  {
741  if(Verbosity()>0) std::cout << "Kept cluster" << std::endl;
742  propagated_track.push_back(next_ckey);
743 // kftrack.SetX(cx*cos(cphi)+cy*sin(cphi));
744 // kftrack.SetY(-cx*sin(cphi)+cy*cos(cphi));
745 // kftrack.SetZ(cz);
746  }
747  else
748  {
749  if(Verbosity()>0)
750  {
751  std::cout << "Rejected cluster" << std::endl;
752  std::cout << "x: " << fabs(tx-cx) << " vs. " << _max_dist*sqrt(txerr*txerr+cxerr*cxerr) << std::endl;
753  std::cout << "y: " << fabs(ty-cy) << " vs. " << _max_dist*sqrt(tyerr*tyerr+cyerr*cyerr) << std::endl;
754  std::cout << "z: " << fabs(tz-cz) << " vs. " << _max_dist*sqrt(tzerr*tzerr+czerr*czerr) << std::endl;
755  }
756  kftrack.SetNDF(kftrack.GetNDF()-2);
757  //ckeys.erase(std::remove(ckeys.begin(),ckeys.end(),next_ckey),ckeys.end());
758  }
759  old_phi = cphi;
760  }
761  // if layer is not occupied, search for the nearest available cluster to projected track position
762  else
763  {
764  if(Verbosity()>0) std::cout << "layer not filled" << std::endl;
765  // get current track coordinates to extract B field from map
766  double tX = kftrack.GetX();
767  double tY = kftrack.GetY();
768  double tx = tX*cos(old_phi)-tY*sin(old_phi);
769  double ty = tX*sin(old_phi)+tY*cos(old_phi);
770  double tz = kftrack.GetZ();
771 // GPUTPCTrackParam::GPUTPCTrackFitParam fp;
772 // kftrack.CalculateFitParameters(fp);
773  kftrack.TransportToX(radii[l-7],kfline,_Bzconst*get_Bz(tx,ty,tz),10.);
774  if(std::isnan(kftrack.GetX()) ||
775  std::isnan(kftrack.GetY()) ||
776  std::isnan(kftrack.GetZ())) continue;
777  // update track coordinates after transport
778  tX = kftrack.GetX();
779  tY = kftrack.GetY();
780  double tYerr = sqrt(kftrack.GetCov(0));
781  double tzerr = sqrt(kftrack.GetCov(5));
782  tx = tX*cos(old_phi)-tY*sin(old_phi);
783  ty = tX*sin(old_phi)+tY*cos(old_phi);
784  tz = kftrack.GetZ();
785  double query_pt[3] = {tx, ty, tz};
786 
787  if(m_dcc)
788  {
789  // The distortion corrected cluster positions in globalPos are not at the layer radius
790  // We want to project to the radius appropriate for the globalPos values
791  // Get the distortion correction for the projection point, and calculate the radial increment
792 
793  double proj_radius = sqrt(tx*tx+ty*ty);
794  if(proj_radius > 78.0 || abs(tz) > 105.5) continue; // projection is bad, no cluster will be found
795 
796  Acts::Vector3D proj_pt(tx,ty,tz);
797  if(Verbosity() > 2)
798  std::cout << " call distortion correction for layer " << l << " tx " << tx << " ty " << ty << " tz " << tz << " radius " << proj_radius << std::endl;
799  proj_pt = m_distortionCorrection.get_corrected_position( proj_pt, m_dcc );
800  // this point is meaningless, except that it gives us an estimate of the corrected radius of a point measured in this layer
801  double radius = sqrt(proj_pt[0]*proj_pt[0] + proj_pt[1]*proj_pt[1]);
802  // now project the track to that radius
803  if(Verbosity() > 2)
804  std::cout << " call transport again for layer " << l << " x " << proj_pt[0] << " y " << proj_pt[1] << " z " << proj_pt[2]
805  << " radius " << radius << std::endl;
806  kftrack.TransportToX(radius,kfline,_Bzconst*get_Bz(tx,ty,tz),10.);
807  if(std::isnan(kftrack.GetX()) ||
808  std::isnan(kftrack.GetY()) ||
809  std::isnan(kftrack.GetZ())) continue;
810  tX = kftrack.GetX();
811  tY = kftrack.GetY();
812  tYerr = sqrt(kftrack.GetCov(0));
813  tzerr = sqrt(kftrack.GetCov(5));
814  tx = tX*cos(old_phi)-tY*sin(old_phi);
815  ty = tX*sin(old_phi)+tY*cos(old_phi);
816  tz = kftrack.GetZ();
817  query_pt[0] = tx;
818  query_pt[1] = ty;
819  query_pt[2] = tz;
820  }
821 
822  double txerr = fabs(tYerr*sin(old_phi));
823  double tyerr = fabs(tYerr*cos(old_phi));
824  if(Verbosity()>0) std::cout << "transported to " << radii[l-7] << "\n";
825  if(Verbosity()>0) std::cout << "track position: (" << tx << ", " << ty << ", " << tz << ")" << std::endl;
826  if(Verbosity()>0) std::cout << "track position error: (" << txerr << ", " << tyerr << ", " << tzerr << ")" << std::endl;
827 
828 // size_t ret_index;
829 // double out_dist_sqr;
830 // nanoflann::KNNResultSet<double> resultSet(1);
831 // resultSet.init(&ret_index,&out_dist_sqr);
832 // _kdtrees[l]->findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
833  std::vector<long unsigned int> index_out(1);
834  std::vector<double> distance_out(1);
835  int n_results = _kdtrees[l]->knnSearch(&query_pt[0],1,&index_out[0],&distance_out[0]);
836  if(Verbosity()>0) std::cout << "index_out: " << index_out[0] << std::endl;
837  if(Verbosity()>0) std::cout << "squared_distance_out: " << distance_out[0] << std::endl;
838  if(Verbosity()>0) std::cout << "solid_angle_dist: " << atan2(sqrt(distance_out[0]),radii[l-7]) << std::endl;
839  if(n_results==0) continue;
840  std::vector<double> point = _ptclouds[l]->pts[index_out[0]];
841  TrkrDefs::cluskey closest_ckey = (*((int64_t*)&point[3]));
842  TrkrCluster* cc = _cluster_map->findCluster(closest_ckey);
843  auto ccglob = globalPositions.at(closest_ckey);
844  double ccX = ccglob(0);
845  double ccY = ccglob(1);
846  double ccZ = ccglob(2);
847 
848  /*
849  // alternatively:
850  if(m_dcc)
851  {
852  // The distortion corrected cluster positions in globalPos are not at the layer radius
853  // We want to project to the radius appropriate for the globalPos values
854  // Get the radius from the nearest associated cluster found above
855  Acts::Vector3D proj_pt(ccX, ccY, ccZ);
856  double radius = sqrt(proj_pt[0]*proj_pt[0] + proj_pt[1]*proj_pt[1]);
857 
858  // now project the track to that radius
859  if(Verbosity() > 2)
860  std::cout << " call transport again for layer " << l << " x " << proj_pt[0] << " y " << proj_pt[1] << " z " << proj_pt[2]
861  << " radius " << radius << std::endl;
862  kftrack.TransportToX(radius,kfline,_Bzconst*get_Bz(ccX,ccY,ccZ),10.);
863  if(std::isnan(kftrack.GetX()) ||
864  std::isnan(kftrack.GetY()) ||
865  std::isnan(kftrack.GetZ())) continue;
866  tX = kftrack.GetX();
867  tY = kftrack.GetY();
868  tYerr = sqrt(kftrack.GetCov(0));
869  tzerr = sqrt(kftrack.GetCov(5));
870  tx = tX*cos(old_phi)-tY*sin(old_phi);
871  ty = tX*sin(old_phi)+tY*cos(old_phi);
872  tz = kftrack.GetZ();
873  query_pt[0] = tx;
874  query_pt[1] = ty;
875  query_pt[2] = tz;
876 
877  n_results = _kdtrees[l]->knnSearch(&query_pt[0],1,&index_out[0],&distance_out[0]);
878  if(Verbosity()>0) std::cout << "index_out: " << index_out[0] << std::endl;
879  if(Verbosity()>0) std::cout << "squared_distance_out: " << distance_out[0] << std::endl;
880  if(Verbosity()>0) std::cout << "solid_angle_dist: " << atan2(sqrt(distance_out[0]),radii[l-7]) << std::endl;
881  if(n_results==0) continue;
882  point = _ptclouds[l]->pts[index_out[0]];
883  closest_ckey = (*((int64_t*)&point[3]));
884  cc = _cluster_map->findCluster(closest_ckey);
885  ccglob = globalPositions.at(closest_ckey);
886  ccX = ccglob(0);
887  ccY = ccglob(1);
888  ccZ = ccglob(2);
889  }
890  */
891 
892  double cxerr = sqrt(fitter->getClusterError(cc,ccglob,0,0));
893  double cyerr = sqrt(fitter->getClusterError(cc,ccglob,1,1));
894  double czerr = sqrt(fitter->getClusterError(cc,ccglob,2,2));
895  double ccphi = atan2(ccY,ccX);
896  if(Verbosity()>0) std::cout << "cluster position: (" << ccX << ", " << ccY << ", " << ccZ << ")" << std::endl;
897  if(Verbosity()>0) std::cout << "cluster position error: (" << cxerr << ", " << cyerr << ", " << czerr << ")" << std::endl;
898  if(Verbosity()>0) std::cout << "cluster X: " << ccX*cos(ccphi)+ccY*sin(ccphi) << std::endl;
899  if(fabs(tx-ccX)<_max_dist*sqrt(txerr*txerr+cxerr*cxerr) &&
900  fabs(ty-ccY)<_max_dist*sqrt(tyerr*tyerr+cyerr*cyerr) &&
901  fabs(tz-ccZ)<_max_dist*sqrt(tzerr*tzerr+czerr*czerr))
902  {
903  propagated_track.push_back(closest_ckey);
904  layers.push_back(TrkrDefs::getLayer(closest_ckey));
905 /* TrkrCluster* cc = _cluster_map->findCluster(closest_ckey);
906  double ccX = cc->getX();
907  std::cout << "cluster X: " << ccX << std::endl;
908  double ccY = cc->getY();
909  double ccx = ccX*cos(old_phi)-ccY*sin(old_phi);
910  double ccy = ccX*sin(old_phi)+ccY*cos(old_phi);
911  std::cout << "cluster position: (" << ccx << ", " << ccy << ", " << cc->getZ() << ")" << std::endl;
912 */
913 
914  double alpha = ccphi-old_phi;
915  kftrack.Rotate(alpha,kfline,10.);
916 // kftrack.SetX(ccX*cos(ccphi)+ccY*sin(ccphi));
917 // kftrack.SetY(-ccX*sin(ccphi)+ccY*cos(ccphi));
918 // kftrack.SetZ(cc->getZ());
919  double ccaY = -ccX*sin(ccphi)+ccY*cos(ccphi);
920  double ccerrY = fitter->getClusterError(cc,ccglob,0,0)*sin(ccphi)*sin(ccphi)+fitter->getClusterError(cc,ccglob,0,1)*sin(ccphi)*cos(ccphi)+fitter->getClusterError(cc,ccglob,1,1)*cos(ccphi)*cos(ccphi);
921  double ccerrZ = fitter->getClusterError(cc,ccglob,2,2);
922  kftrack.Filter(ccaY,ccZ,ccerrY,ccerrZ,_max_sin_phi);
923  if(Verbosity()>0) std::cout << "added cluster" << std::endl;
924  old_phi = ccphi;
925  }
926  }
927  old_layer = l;
928  }
929 // old_layer = TrkrDefs::getLayer(ckeys[0]);
930 // std::reverse(ckeys.begin(),ckeys.end());
931  layers.clear();
932  if(Verbosity()>0) std::cout << "\nlayers after outward propagation:" << std::endl;
933  for(int i=0;i<propagated_track.size();i++)
934  {
935  layers.push_back(TrkrDefs::getLayer(propagated_track[i]));
936  if(Verbosity()>0) std::cout << layers[i] << std::endl;
937  }
938  // then, propagate upward
939  for(unsigned int l=old_layer-1;l>=7;l--)
940  {
941  if(std::isnan(kftrack.GetX()) ||
942  std::isnan(kftrack.GetY()) ||
943  std::isnan(kftrack.GetZ())) continue;
944  if(Verbosity()>0) std::cout << "\nlayer " << l << ":" << std::endl;
945  // check to see whether layer is already occupied by at least one cluster
946  // choosing the first one first (clusters organized from outside in)
947  bool layer_filled = false;
948  TrkrDefs::cluskey next_ckey = 0;
949  for(size_t k=0; k<layers.size(); k++)
950  {
951  if(layer_filled) continue;
952  if(layers[k]==l)
953  {
954  layer_filled = true;
955  next_ckey = propagated_track[k];
956  }
957  }
958  // if layer is already occupied, reset track parameters to last cluster in layer
959  if(layer_filled)
960  {
961  if(Verbosity()>0) std::cout << "layer is filled" << std::endl;
962  auto ncglob = globalPositions.at(next_ckey);
963  double cx = ncglob(0);
964  double cy = ncglob(1);
965  double cz = ncglob(2);
966  double cphi = atan2(cy,cx);
967  double alpha = cphi-old_phi;
968  double tX = kftrack.GetX();
969  double tY = kftrack.GetY();
970  double tx = tX*cos(old_phi)-tY*sin(old_phi);
971  double ty = tX*sin(old_phi)+tY*cos(old_phi);
972  double tz = kftrack.GetZ();
973 // GPUTPCTrackParam::GPUTPCTrackFitParam fp;
974 // kftrack_up.CalculateFitParameters(fp);
975  kftrack.Rotate(alpha,kfline,10.);
976  kftrack.TransportToX(cx*cos(cphi)+cy*sin(cphi),kfline,_Bzconst*get_Bz(tx,ty,tz),10.);
977  if(std::isnan(kftrack.GetX()) ||
978  std::isnan(kftrack.GetY()) ||
979  std::isnan(kftrack.GetZ())) continue;
980  if(Verbosity()>0) std::cout << "transported to " << radii[l-7] << "\n";
981  if(Verbosity()>0) std::cout << "track position: (" << kftrack.GetX()*cos(cphi)-kftrack.GetY()*sin(cphi) << ", " << kftrack.GetX()*sin(cphi)+kftrack.GetY()*cos(cphi) << ", " << kftrack.GetZ() << ")" << std::endl;
982  if(Verbosity()>0) std::cout << "cluster position: (" << cx << ", " << cy << ", " << cz << ")" << std::endl;
983 // kftrack.SetX(cx*cos(cphi)+cy*sin(cphi));
984 // kftrack.SetY(-cx*sin(cphi)+cy*cos(cphi));
985 // kftrack.SetZ(cz);
986 // propagated_track.push_back(next_ckey);
987  old_phi = cphi;
988  }
989  // if layer is not occupied, search for the nearest available cluster to projected track position
990  else
991  {
992  if(Verbosity()>0) std::cout << "layer not filled" << std::endl;
993  double tX = kftrack.GetX();
994  double tY = kftrack.GetY();
995  double tx = tX*cos(old_phi)-tY*sin(old_phi);
996  double ty = tX*sin(old_phi)+tY*cos(old_phi);
997  double tz = kftrack.GetZ();
998 // GPUTPCTrackParam::GPUTPCTrackFitParam fp;
999 // kftrack_up.CalculateFitParameters(fp);
1000  kftrack.TransportToX(radii[l-7],kfline,_Bzconst*get_Bz(tx,ty,tz),10.);
1001  if(std::isnan(kftrack.GetX()) ||
1002  std::isnan(kftrack.GetY()) ||
1003  std::isnan(kftrack.GetZ())) continue;
1004  tX = kftrack.GetX();
1005  tY = kftrack.GetY();
1006  double tYerr = sqrt(kftrack.GetCov(0));
1007  double tzerr = sqrt(kftrack.GetCov(5));
1008  tx = tX*cos(old_phi)-tY*sin(old_phi);
1009  ty = tX*sin(old_phi)+tY*cos(old_phi);
1010  tz = kftrack.GetZ();
1011  double query_pt[3] = {tx, ty, tz};
1012 
1013  // Now look for the nearest cluster to this projection point (tx,ty,tz), which is at the nominal layer radius
1014  if(m_dcc)
1015  {
1016  // The distortion corrected cluster positions in globalPos are not at the layer radius
1017  // We want to project to the radius appropriate for the globalPos values
1018  // Get the distortion correction for the projection point, and calculate the radial increment
1019  double proj_radius = sqrt(tx*tx+ty*ty);
1020  if(proj_radius > 78.0 || abs(tz) > 105.5) continue; // projection is bad, no cluster will be found
1021 
1022  Acts::Vector3D proj_pt(tx,ty,tz);
1023  if(Verbosity() > 2)
1024  std::cout << " call distortion correction for layer " << l << " tx " << tx << " ty " << ty << " tz " << tz << " radius " << proj_radius << std::endl;
1025  proj_pt = m_distortionCorrection.get_corrected_position( proj_pt, m_dcc );
1026  // this point is meaningless, except that it givs us an estimate of the corrected radius of a point measured in this layer
1027  double radius = sqrt(proj_pt[0]*proj_pt[0] + proj_pt[1]*proj_pt[1]);
1028 
1029  // now project the track to that radius
1030  if(Verbosity() > 2)
1031  std::cout << " call transport again for layer " << l << " x " << proj_pt[0] << " y " << proj_pt[1] << " z " << proj_pt[2]
1032  << " radius " << radius << std::endl;
1033  kftrack.TransportToX(radius,kfline,_Bzconst*get_Bz(tx,ty,tz),10.);
1034  if(std::isnan(kftrack.GetX()) ||
1035  std::isnan(kftrack.GetY()) ||
1036  std::isnan(kftrack.GetZ())) continue;
1037  tX = kftrack.GetX();
1038  tY = kftrack.GetY();
1039  tYerr = sqrt(kftrack.GetCov(0));
1040  tzerr = sqrt(kftrack.GetCov(5));
1041  tx = tX*cos(old_phi)-tY*sin(old_phi);
1042  ty = tX*sin(old_phi)+tY*cos(old_phi);
1043  tz = kftrack.GetZ();
1044  query_pt[0] = tx;
1045  query_pt[1] = ty;
1046  query_pt[2] = tz;
1047  }
1048 
1049  double txerr = fabs(tYerr*sin(old_phi));
1050  double tyerr = fabs(tYerr*cos(old_phi));
1051  if(Verbosity()>0) std::cout << "transported to " << radii[l-7] << "\n";
1052  if(Verbosity()>0) std::cout << "track position: (" << kftrack.GetX()*cos(old_phi)-kftrack.GetY()*sin(old_phi) << ", " << kftrack.GetX()*sin(old_phi)+kftrack.GetY()*cos(old_phi) << ", " << kftrack.GetZ() << ")" << std::endl;
1053  if(Verbosity()>0) std::cout << "track position errors: (" << txerr << ", " << tyerr << ", " << tzerr << ")" << std::endl;
1054 
1055  std::vector<long unsigned int> index_out(1);
1056  std::vector<double> distance_out(1);
1057  int n_results = _kdtrees[l]->knnSearch(&query_pt[0],1,&index_out[0],&distance_out[0]);
1058  if(Verbosity()>0) std::cout << "index_out: " << index_out[0] << std::endl;
1059  if(Verbosity()>0) std::cout << "squared_distance_out: " << distance_out[0] << std::endl;
1060  if(Verbosity()>0) std::cout << "solid_angle_dist: " << atan2(sqrt(distance_out[0]),radii[l-7]) << std::endl;
1061  if(n_results==0) continue;
1062  std::vector<double> point = _ptclouds[l]->pts[index_out[0]];
1063  TrkrDefs::cluskey closest_ckey = (*((int64_t*)&point[3]));
1064  TrkrCluster* cc = _cluster_map->findCluster(closest_ckey);
1065  auto ccglob2 = globalPositions.at(closest_ckey);
1066  double ccX = ccglob2(0);
1067  double ccY = ccglob2(1);
1068  double ccZ = ccglob2(2);
1069 
1070  /*
1071  // alternatively:
1072  if(m_dcc)
1073  {
1074  // The distortion corrected cluster positions in globalPos are not at the layer radius
1075  // We want to project to the radius appropriate for the globalPos values
1076  // Get the radius from the global position of the nearest cluster found above
1077  Acts::Vector3D proj_pt(ccX, ccY, ccZ);
1078  double radius = sqrt(proj_pt[0]*proj_pt[0] + proj_pt[1]*proj_pt[1]);
1079 
1080  // now project the track to that radius
1081  if(Verbosity() > 2)
1082  std::cout << " call transport again for layer " << l << " x " << proj_pt[0] << " y " << proj_pt[1] << " z " << proj_pt[2]
1083  << " radius " << radius << std::endl;
1084  kftrack.TransportToX(radius,kfline,_Bzconst*get_Bz(ccX,ccY,ccZ),10.);
1085  if(std::isnan(kftrack.GetX()) ||
1086  std::isnan(kftrack.GetY()) ||
1087  std::isnan(kftrack.GetZ())) continue;
1088  tX = kftrack.GetX();
1089  tY = kftrack.GetY();
1090  tYerr = sqrt(kftrack.GetCov(0));
1091  tzerr = sqrt(kftrack.GetCov(5));
1092  tx = tX*cos(old_phi)-tY*sin(old_phi);
1093  ty = tX*sin(old_phi)+tY*cos(old_phi);
1094  tz = kftrack.GetZ();
1095  query_pt[0] = tx;
1096  query_pt[1] = ty;
1097  query_pt[2] = tz;
1098 
1099  n_results = _kdtrees[l]->knnSearch(&query_pt[0],1,&index_out[0],&distance_out[0]);
1100  if(Verbosity()>0) std::cout << "index_out: " << index_out[0] << std::endl;
1101  if(Verbosity()>0) std::cout << "squared_distance_out: " << distance_out[0] << std::endl;
1102  if(Verbosity()>0) std::cout << "solid_angle_dist: " << atan2(sqrt(distance_out[0]),radii[l-7]) << std::endl;
1103  if(n_results==0) continue;
1104  point = _ptclouds[l]->pts[index_out[0]];
1105  closest_ckey = (*((int64_t*)&point[3]));
1106  cc = _cluster_map->findCluster(closest_ckey);
1107  ccglob2 = globalPositions.at(closest_ckey);
1108  ccX = ccglob2(0);
1109  ccY = ccglob2(1);
1110  ccZ = ccglob2(2);
1111  }
1112  */
1113 
1114  double cxerr = sqrt(fitter->getClusterError(cc,ccglob2,0,0));
1115  double cyerr = sqrt(fitter->getClusterError(cc,ccglob2,1,1));
1116  double czerr = sqrt(fitter->getClusterError(cc,ccglob2,2,2));
1117  if(Verbosity()>0) std::cout << "cluster position: (" << ccX << ", " << ccY << ", " << ccZ << ")" << std::endl;
1118  double ccphi = atan2(ccY,ccX);
1119  if(Verbosity()>0) std::cout << "cluster position errors: (" << cxerr << ", " << cyerr << ", " << czerr << ")" << std::endl;
1120  if(Verbosity()>0) std::cout << "cluster X: " << ccX*cos(ccphi)+ccY*sin(ccphi) << std::endl;
1121  double alpha = ccphi-old_phi;
1122  if(fabs(tx-ccX)<_max_dist*sqrt(txerr*txerr+cxerr*cxerr) &&
1123  fabs(ty-ccY)<_max_dist*sqrt(tyerr*tyerr+cyerr*cyerr) &&
1124  fabs(tz-ccZ)<_max_dist*sqrt(tzerr*tzerr+czerr*czerr))
1125  {
1126  propagated_track.push_back(closest_ckey);
1127  layers.push_back(TrkrDefs::getLayer(closest_ckey));
1128 /* TrkrCluster* cc = _cluster_map->findCluster(closest_ckey);
1129  double ccX = cc->getX();
1130  std::cout << "cluster X: " << ccX << std::endl;
1131  double ccY = cc->getY();
1132  double ccx = ccX*cos(old_phi)-ccY*sin(old_phi);
1133  double ccy = ccX*sin(old_phi)+ccY*cos(old_phi);
1134  std::cout << "cluster position: (" << ccx << ", " << ccy << ", " << cc->getZ() << ")" << std::endl;
1135  double ccphi = atan2(ccy,ccx);
1136  double alpha = ccphi-old_phi;
1137 */
1138  kftrack.Rotate(alpha,kfline,10.);
1139  double ccaY = -ccX*sin(ccphi)+ccY*cos(ccphi);
1140  double ccerrY = fitter->getClusterError(cc,ccglob2,0,0)*sin(ccphi)*sin(ccphi)+fitter->getClusterError(cc,ccglob2,1,0)*sin(ccphi)*cos(ccphi)+fitter->getClusterError(cc,ccglob2,1,1)*cos(ccphi)*cos(ccphi);
1141  double ccerrZ = fitter->getClusterError(cc,ccglob2,2,2);
1142  kftrack.Filter(ccaY,ccZ,ccerrY,ccerrZ,_max_sin_phi);
1143 // kftrack.SetX(ccX*cos(ccphi)+ccY*sin(ccphi));
1144 // kftrack.SetY(-ccX*sin(ccphi)+ccY*cos(ccphi));
1145 // kftrack.SetZ(cc->getZ());
1146  if(Verbosity()>0) std::cout << "added cluster" << std::endl;
1147  old_phi = ccphi;
1148  }
1149  }
1150  old_layer = l;
1151  }
1152  std::sort(propagated_track.begin(),propagated_track.end(),
1154  {return (TrkrDefs::getLayer(a)<TrkrDefs::getLayer(b));});
1155  return propagated_track;
1156 }
1157 
1158 std::vector<keylist> PHSimpleKFProp::RemoveBadClusters(const std::vector<keylist>& chains, const PositionMap& globalPositions) const
1159 {
1160  if(Verbosity()>0) std::cout << "removing bad clusters" << std::endl;
1161  std::vector<keylist> clean_chains;
1162  for(const keylist& chain : chains)
1163  {
1164  if(chain.size()<3) continue;
1165  keylist clean_chain;
1166 
1167  std::vector<std::pair<double,double>> xy_pts;
1168  std::vector<std::pair<double,double>> rz_pts;
1169  for(const TrkrDefs::cluskey& ckey : chain)
1170  {
1171  auto global = globalPositions.at(ckey);
1172  xy_pts.push_back(std::make_pair(global(0),global(1)));
1173  float r = sqrt(global(0)*global(0) + global(1)*global(1));
1174  rz_pts.push_back(std::make_pair(r,global(2)));
1175  }
1176  if(Verbosity()>0) std::cout << "chain size: " << chain.size() << std::endl;
1177  double A;
1178  double B;
1179  double R;
1180  double X0;
1181  double Y0;
1182  fitter->CircleFitByTaubin(xy_pts,R,X0,Y0);
1183  fitter->line_fit(rz_pts,A,B);
1184  std::vector<double> xy_resid = fitter->GetCircleClusterResiduals(xy_pts,R,X0,Y0);
1185  std::vector<double> rz_resid = fitter->GetLineClusterResiduals(rz_pts,A,B);
1186  for(size_t i=0;i<chain.size();i++)
1187  {
1188  if(xy_resid[i]>_xy_outlier_threshold) continue;
1189  clean_chain.push_back(chain[i]);
1190  }
1191  clean_chains.push_back(clean_chain);
1192  if(Verbosity()>0) std::cout << "pushed clean chain with " << clean_chain.size() << " clusters" << std::endl;
1193  }
1194  return clean_chains;
1195 }
1196 
1197 
1198 
1199 void PHSimpleKFProp::publishSeeds(const std::vector<SvtxTrack_v2>& seeds)
1200 {
1201  for( const auto& seed:seeds )
1202  { _track_map->insert(&seed); }
1203 }
1204 
1205 void PHSimpleKFProp::publishSeeds(const std::vector<SvtxTrack>& seeds)
1206 {
1207  for( const auto& seed:seeds )
1208  { _track_map->insert(&seed); }
1209 }
1210 
1211 // void PHSimpleKFProp::MoveToVertex()
1212 // {
1213 // // _track_map contains the TPC seed track stubs
1214 // // We want to associate these TPC track seeds with a collision vertex
1215 // // Then we add the collision vertex position as the track seed position
1216 //
1217 // // All we need is to project the TPC clusters in Z to the beam line.
1218 //
1219 //
1220 // if(Verbosity() > 0)
1221 // std::cout << PHWHERE << " TPC track map size " << _track_map->size() << std::endl;
1222 // /*
1223 // // We remember the original size of the TPC track map here
1224 // const unsigned int original_track_map_lastkey = _track_map->empty() ? 0:std::prev(_track_map->end())->first;
1225 // */
1226 //
1227 // // loop over the TPC track seeds
1228 // for (auto phtrk_iter = _track_map->begin();
1229 // phtrk_iter != _track_map->end();
1230 // ++phtrk_iter)
1231 // {
1232 // /*
1233 // // we may add tracks to the map, so we stop at the last original track
1234 // if(phtrk_iter->first >= original_track_map_lastkey) break;
1235 // */
1236 // SvtxTrack* _tracklet_tpc = phtrk_iter->second;
1237 //
1238 // if (Verbosity() >= 1)
1239 // {
1240 // std::cout
1241 // << __LINE__
1242 // << ": Processing seed itrack: " << phtrk_iter->first
1243 // << ": nhits: " << _tracklet_tpc-> size_cluster_keys()
1244 // << ": phi: " << _tracklet_tpc->get_phi()
1245 // << ": eta: " << _tracklet_tpc->get_eta()
1246 // << std::endl;
1247 // }
1248 //
1249 // // get the tpc track seed cluster positions in z and r
1250 //
1251 // // Get the outermost TPC clusters for this tracklet
1252 // std::map<unsigned int, TrkrCluster*> tpc_clusters_map;
1253 // std::vector<TrkrCluster*> clusters;
1254 //
1255 // for (SvtxTrack::ConstClusterKeyIter key_iter = _tracklet_tpc->begin_cluster_keys();
1256 // key_iter != _tracklet_tpc->end_cluster_keys();
1257 // ++key_iter)
1258 // {
1259 // TrkrDefs::cluskey cluster_key = *key_iter;
1260 // unsigned int layer = TrkrDefs::getLayer(cluster_key);
1261 //
1262 // //if(layer < _min_tpc_layer) continue;
1263 // //if(layer >= _max_tpc_layer) continue;
1264 //
1265 // // get the cluster
1266 // TrkrCluster *tpc_clus = _cluster_map->findCluster(cluster_key);
1267 //
1268 // tpc_clusters_map.insert(std::make_pair(layer, tpc_clus));
1269 // clusters.push_back(tpc_clus);
1270 //
1271 // if(Verbosity() > 5)
1272 // std::cout << " TPC cluster in layer " << layer << " with local position " << tpc_clus->getLocalX()
1273 // << " " << tpc_clus->getLocalY() << " clusters.size() " << tpc_clusters_map.size() << std::endl;
1274 // }
1275 //
1276 //
1277 // // need at least 3 clusters to fit a circle
1278 // if(tpc_clusters_map.size() < 3)
1279 // {
1280 // if(Verbosity() > 3) std::cout << PHWHERE << " -- skip this tpc tracklet, not enough clusters " << std::endl;
1281 // continue; // skip to the next TPC tracklet
1282 // }
1283 //
1284 // /*
1285 // // fit a circle to the clusters
1286 // double R, X0, Y0;
1287 // CircleFitByTaubin(clusters, R, X0, Y0);
1288 // if(Verbosity() > 10) std::cout << " Fitted circle has R " << R << " X0 " << X0 << " Y0 " << Y0 << std::endl;
1289 // // toss tracks for which the fitted circle could not have come from the vertex
1290 // if(R < 40.0) continue;
1291 // */
1292 //
1293 // // get the straight line representing the z trajectory in the form of z vs radius
1294 // double A = 0; double B = 0;
1295 // line_fit_clusters(clusters, A, B);
1296 // if(Verbosity() > 5) std::cout << " Fitted line has A " << A << " B " << B << std::endl;
1297 //
1298 // // Project this TPC tracklet to the beam line and store the projections
1299 // //bool skip_tracklet = false;
1300 // // z projection is unique
1301 // double _z_proj = B;
1302 //
1303 // // // Now we modify the track parameters
1304 // // // Repeat the z line fit including the vertex position, get theta, update pz
1305 // // std::vector<std::pair<double, double>> points;
1306 // // std::transform( clusters.begin(), clusters.end(), std::back_inserter( points ), []( TrkrCluster* cluster )
1307 // // {
1308 // // double z = cluster->getZ();
1309 // // double r = std::sqrt(square(cluster->getX()) + square(cluster->getY()));
1310 // // return std::make_pair(r,z);
1311 // // } );
1312 //
1313 // // Now we modify the track parameters
1314 // // Repeat the z line fit including the vertex position, get theta, update pz
1315 // //TODO: check - I don't think the code does the above. Just redo the same fit a second time, in which case it can be commented
1316 // std::vector<std::pair<double, double>> points;
1317 // for (unsigned int i=0; i<clusters.size(); ++i)
1318 // {
1319 // double z = clusters[i]->getZ();
1320 // double r = std::sqrt(square(clusters[i]->getX()) + square(clusters[i]->getY()));
1321 // points.push_back(std::make_pair(r,z));
1322 // }
1323 //
1324 // line_fit(points, A, B);
1325 //
1326 // if(Verbosity() > 5)
1327 // std::cout << " Fitted line including vertex has A " << A << " B " << B << std::endl;
1328 //
1329 // // extract the track theta
1330 // double track_angle = atan(A); // referenced to 90 degrees
1331 //
1332 // // update pz of track
1333 // double pt_track = _tracklet_tpc->get_pt();
1334 // double ptrack = sqrt(pt_track*pt_track + _tracklet_tpc->get_pz()*_tracklet_tpc->get_pz());
1335 // double pz_new = ptrack * sin(track_angle);
1336 // if(Verbosity() > 5)
1337 // std::cout << " Original pz = " << _tracklet_tpc->get_pz() << " new pz " << pz_new << " track angle " << track_angle << std::endl;
1338 // _tracklet_tpc->set_pz(pz_new);
1339 // if(Verbosity() > 5)
1340 // std::cout << " new eta " << _tracklet_tpc->get_eta() << std::endl;
1341 // // total momentum is now a bit different because pt was not changed - OK - we measure pt from bend, pz from dz/dr
1342 //
1343 // // make circle fit
1344 // std::vector<std::pair<double, double>> cpoints;
1345 // std::vector<Acts::Vector3F> globalpositions;
1346 // ActsTransformations transformer;
1347 // for (unsigned int i=0; i<clusters.size(); ++i)
1348 // {
1349 // auto glob = transformer.getGlobalPositionF(clusters.at(i),_surfmaps,_tgeometry);
1350 // globalpositions.push_back(glob);
1351 // cpoints.push_back(std::make_pair(glob(0),glob(1)));
1352 // }
1353 // double R, X0, Y0;
1354 // CircleFitByTaubin(globalpositions, R, X0, Y0);
1355 // if(Verbosity() > 5)
1356 // std::cout << " Fitted circle has R " << R << " X0 " << X0 << " Y0 " << Y0 << std::endl;
1357 //
1358 // // set the track x and y positions to the circle PCA
1359 // double dcax, dcay;
1360 // findRoot(R, X0, Y0, dcax, dcay);
1361 // _tracklet_tpc->set_x(dcax);
1362 // _tracklet_tpc->set_y(dcay);
1363 // _tracklet_tpc->set_z(_z_proj);
1364 //
1365 // // could take new pT from radius of circle - we choose to keep the seed pT
1366 //
1367 // // We want the angle of the tangent relative to the positive x axis
1368 // // start with the angle of the radial line from vertex to circle center
1369 // double dx = X0 - dcax;
1370 // double dy = Y0 - dcay;
1371 // double phi= atan2(dy,dx);
1372 // //std::cout << "x_vertex " << x_vertex << " y_vertex " << y_vertex << " X0 " << X0 << " Y0 " << Y0 << " angle " << phi * 180 / 3.14159 << std::endl;
1373 // // convert to the angle of the tangent to the circle
1374 // // we need to know if the track proceeds clockwise or CCW around the circle
1375 // double dx0 = cpoints[0].first - X0;
1376 // double dy0 = cpoints[0].second - Y0;
1377 // double phi0 = atan2(dy0, dx0);
1378 // double dx1 = cpoints[1].first - X0;
1379 // double dy1 = cpoints[1].second - Y0;
1380 // double phi1 = atan2(dy1, dx1);
1381 // double dphi = phi1 - phi0;
1382 //
1383 // if(Verbosity() > 5)
1384 // {
1385 // int charge = _tracklet_tpc->get_charge(); // needed for diagnostic output only
1386 // std::cout << " charge " << charge << " phi0 " << phi0*180.0 / M_PI << " phi1 " << phi1*180.0 / M_PI << " dphi " << dphi*180.0 / M_PI << std::endl;
1387 // }
1388 //
1389 // // whether we add or subtract 90 degrees depends on the track propagation direction determined above
1390 // if(dphi < 0)
1391 // phi += M_PI / 2.0;
1392 // else
1393 // phi -= M_PI / 2.0;
1394 // if(Verbosity() > 5)
1395 // std::cout << " input track phi " << _tracklet_tpc->get_phi() * 180.0 / M_PI << " new phi " << phi * 180 / M_PI << std::endl;
1396 //
1397 // // update px, py of track
1398 // double px_new = pt_track * cos(phi);
1399 // double py_new = pt_track * sin(phi);
1400 // if(Verbosity() > 5)
1401 // std::cout << " input track px " << _tracklet_tpc->get_px() << " new px " << px_new << " input py " << _tracklet_tpc->get_py() << " new py " << py_new << std::endl;
1402 //
1403 // // update track on node tree
1404 // _tracklet_tpc->set_px(px_new);
1405 // _tracklet_tpc->set_py(py_new);
1406 //
1407 // } // end loop over TPC track seeds
1408 // }