20 #include <phfield/PHField.h>
21 #include <phfield/PHFieldUtility.h>
39 #include <Geant4/G4SystemOfUnits.hh>
42 #include <Eigen/Dense>
50 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp
52 #define LogDebug(exp) (void)0
55 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp
56 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp
62 template<
class T>
inline constexpr
T square(
const T&
x ) {
return x*
x; }
103 void CircleFitByTaubin (
const std::vector<Acts::Vector3F>& points,
double &
R,
double &X0,
double &Y0)
115 double Mz,Mxy,Mxx,Myy,Mxz,Myz,Mzz,Cov_xy,Var_z;
116 double A0,A1,A2,
A22,A3,
A33;
118 double DET,Xcenter,Ycenter;
124 for(
const auto&
point:points )
135 Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;
136 for(
const auto&
point:points )
138 double Xi =
point(0) - meanX;
139 double Yi =
point(1) - meanY;
140 double Zi = Xi*Xi + Yi*Yi;
157 Cov_xy = Mxx*Myy - Mxy*Mxy;
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;
170 for (x=0.,y=A0,iter=0; iter<IterMAX; iter++)
172 double Dy = A1 + x*(A22 + A33*
x);
173 double xnew = x - y/
Dy;
175 double ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
176 if (fabs(ynew)>=fabs(y))
break;
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;
188 X0 = Xcenter + meanX;
189 Y0 = Ycenter + meanY;
234 using keylist = std::vector<TrkrDefs::cluskey>;
251 _surfmaps = findNode::getClass<ActsSurfaceMaps>(topNode,
"ActsSurfaceMaps");
254 std::cout <<
"No Acts surface maps, exiting." << std::endl;
258 _tgeometry = findNode::getClass<ActsTrackingGeometry>(topNode,
"ActsTrackingGeometry");
261 std::cout <<
"No Acts tracking geometry, exiting." << std::endl;
266 m_dcc = findNode::getClass<TpcDistortionCorrectionContainer>(topNode,
"TpcDistortionCorrectionContainer");
268 { std::cout <<
"PHSimpleKFProp::InitRun - found TPC distortion correction container" << std::endl; }
284 double p[4] = {x*
cm,y*
cm,z*
cm,0.*cm};
287 return bfield[2]/
tesla;
297 _cluster_map = findNode::getClass<TrkrClusterContainer>(topNode,
"TRKR_CLUSTER_TRUTH");
299 _cluster_map = findNode::getClass<TrkrClusterContainer>(topNode,
"TRKR_CLUSTER");
303 std::cerr <<
PHWHERE <<
" ERROR: Can't find node TRKR_CLUSTER" << std::endl;
307 _track_map = findNode::getClass<SvtxTrackMap>(topNode,
"SvtxTrackMap");
310 std::cerr <<
PHWHERE <<
" ERROR: Can't find SvtxTrackMap " << std::endl;
314 _hitsets = findNode::getClass<TrkrHitSetContainer>(topNode,
"TRKR_HITSET");
317 std::cerr <<
PHWHERE <<
"No hitset container on node tree. Bailing."
323 findNode::getClass<PHG4CylinderCellGeomContainer>(topNode,
"CYLINDERCELLGEOM_SVTX");
326 std::cerr <<
PHWHERE <<
"ERROR: Can't find node CYLINDERCELLGEOM_SVTX" << std::endl;
330 for(
int i=7;i<=54;i++)
341 _iteration_map = findNode::getClass<TrkrClusterIterationMapv1>(topNode,
"CLUSTER_ITERATION_MAP");
343 std::cerr <<
PHWHERE <<
"Cluster Iteration Map missing, aborting." << std::endl;
348 if(
Verbosity()>0) std::cout <<
"starting Process" << std::endl;
350 if(
Verbosity()>0) std::cout <<
"prepared KD trees" << std::endl;
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;
359 const bool is_tpc = std::any_of(
366 if(
Verbosity()>0) std::cout <<
"is tpc track" << std::endl;
372 if(
Verbosity()>0) std::cout <<
"is NOT tpc track" << std::endl;
373 unused_tracks.push_back(*track);
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);
402 std::vector<std::vector<std::vector<double> > > kdhits;
406 std::cout <<
"WARNING: (tracking.PHTpcTrackerUtil.convert_clusters_to_hits) cluster map is not provided" << std::endl;
407 return globalPositions;
411 for (
auto hitsetitr = hitsetrange.first; hitsetitr != hitsetrange.second; ++hitsetitr)
418 if(!cluster)
continue;
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));
434 std::vector<double> kdhit(4);
435 kdhit[0] = globalpos_d.x();
436 kdhit[1] = globalpos_d.y();
437 kdhit[2] = globalpos_d.z();
439 std::memcpy(&kdhit[3], &key,
sizeof(key));
446 kdhits[
layer].push_back(kdhit);
451 for(
size_t l=0;l<kdhits.size();++l)
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)
465 return globalPositions;
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.)
476 if(
Verbosity()>0) std::cout <<
"WARNING: attempting to move track to TPC which is already in TPC! Aborting for this track." << std::endl;
481 std::vector<TrkrDefs::cluskey> ckeys;
482 std::copy(
track->begin_cluster_keys(),
track->end_cluster_keys(),std::back_inserter(ckeys));
484 std::vector<Acts::Vector3F> trkGlobPos;
485 for(
const auto& ckey : ckeys)
488 { trkGlobPos.push_back(globalPositions.at(ckey)); }
496 CircleFitByTaubin(trkGlobPos,R,xc,yc);
501 inner_index = ckeys.size()-1;
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;
525 track->set_px(pt*cos(phi));
526 track->set_py(pt*sin(phi));
528 track->set_x(trkGlobPos.at(0)(0));
529 track->set_y(trkGlobPos.at(0)(1));
530 track->set_z(trkGlobPos.at(0)(2));
538 std::vector<TrkrDefs::cluskey> ckeys;
547 float track_phi = atan2(track->
get_py(),track->
get_px());
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);
554 Eigen::Matrix<double,6,6> xyzCov;
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. );
574 Eigen::Matrix<double,6,5> Jrot;
592 Jrot(3,2) = -track_py*track_px/track_pt3;
593 Jrot(4,2) = track_px*track_px/track_pt3;
599 Jrot(3,3) = -track_px*track_pz/track_pt3;
600 Jrot(4,3) = -track_py*track_pz/track_pt3;
601 Jrot(5,3) = 1./track_pt;
606 Jrot(3,4) = -track_px/track_pt3;
607 Jrot(4,4) = -track_py/track_pt3;
610 Eigen::Matrix<double,5,5> kfCov = Jrot.transpose()*xyzCov*Jrot;
619 kftrack.
SetCov(ctr,kfCov(i,j));
625 std::vector<TrkrDefs::cluskey> propagated_track;
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));
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;
666 std::vector<unsigned int> layers;
667 if(
Verbosity()>0) std::cout <<
"cluster layers:" << std::endl;
670 double old_phi = track_phi;
672 if(
Verbosity()>0) std::cout <<
"first layer: " << old_layer << std::endl;
674 propagated_track.push_back(ckeys[0]);
676 for(
unsigned int l=old_layer+1;l<=54;l++)
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;
685 bool layer_filled =
false;
687 for(
int k=layers.size()-1;
k>=0;
k--)
689 if(layer_filled)
continue;
693 next_ckey = ckeys[
k];
699 if(
Verbosity()>0) std::cout <<
"layer is filled" << std::endl;
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();
717 if(
Verbosity()>0) std::cout <<
"track position: (" << tx <<
", " << ty <<
", " << tz <<
")" << std::endl;
718 kftrack.
Rotate(alpha,kfline,10.);
720 if(std::isnan(kftrack.
GetX()) ||
721 std::isnan(kftrack.
GetY()) ||
722 std::isnan(kftrack.
GetZ()))
continue;
725 tx = tX*cos(cphi)-tY*sin(cphi);
726 ty = tX*sin(cphi)+tY*cos(cphi);
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;
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))
741 if(
Verbosity()>0) std::cout <<
"Kept cluster" << std::endl;
742 propagated_track.push_back(next_ckey);
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;
764 if(
Verbosity()>0) std::cout <<
"layer not filled" << std::endl;
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();
774 if(std::isnan(kftrack.
GetX()) ||
775 std::isnan(kftrack.
GetY()) ||
776 std::isnan(kftrack.
GetZ()))
continue;
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);
785 double query_pt[3] = {tx, ty, tz};
793 double proj_radius = sqrt(tx*tx+ty*ty);
794 if(proj_radius > 78.0 ||
abs(tz) > 105.5)
continue;
798 std::cout <<
" call distortion correction for layer " << l <<
" tx " << tx <<
" ty " << ty <<
" tz " << tz <<
" radius " << proj_radius << std::endl;
801 double radius = sqrt(proj_pt[0]*proj_pt[0] + proj_pt[1]*proj_pt[1]);
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;
807 if(std::isnan(kftrack.
GetX()) ||
808 std::isnan(kftrack.
GetY()) ||
809 std::isnan(kftrack.
GetZ()))
continue;
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);
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;
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;
843 auto ccglob = globalPositions.at(closest_ckey);
844 double ccX = ccglob(0);
845 double ccY = ccglob(1);
846 double ccZ = ccglob(2);
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))
903 propagated_track.push_back(closest_ckey);
914 double alpha = ccphi-old_phi;
915 kftrack.
Rotate(alpha,kfline,10.);
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);
923 if(
Verbosity()>0) std::cout <<
"added cluster" << std::endl;
932 if(
Verbosity()>0) std::cout <<
"\nlayers after outward propagation:" << std::endl;
933 for(
int i=0;i<propagated_track.size();i++)
936 if(
Verbosity()>0) std::cout << layers[i] << std::endl;
939 for(
unsigned int l=old_layer-1;l>=7;l--)
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;
947 bool layer_filled =
false;
949 for(
size_t k=0;
k<layers.size();
k++)
951 if(layer_filled)
continue;
955 next_ckey = propagated_track[
k];
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();
975 kftrack.
Rotate(alpha,kfline,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;
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();
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};
1019 double proj_radius = sqrt(tx*tx+ty*ty);
1020 if(proj_radius > 78.0 ||
abs(tz) > 105.5)
continue;
1024 std::cout <<
" call distortion correction for layer " << l <<
" tx " << tx <<
" ty " << ty <<
" tz " << tz <<
" radius " << proj_radius << std::endl;
1027 double radius = sqrt(proj_pt[0]*proj_pt[0] + proj_pt[1]*proj_pt[1]);
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;
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();
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;
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;
1065 auto ccglob2 = globalPositions.at(closest_ckey);
1066 double ccX = ccglob2(0);
1067 double ccY = ccglob2(1);
1068 double ccZ = ccglob2(2);
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))
1126 propagated_track.push_back(closest_ckey);
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);
1146 if(
Verbosity()>0) std::cout <<
"added cluster" << std::endl;
1152 std::sort(propagated_track.begin(),propagated_track.end(),
1155 return propagated_track;
1160 if(
Verbosity()>0) std::cout <<
"removing bad clusters" << std::endl;
1161 std::vector<keylist> clean_chains;
1162 for(
const keylist& chain : chains)
1164 if(chain.size()<3)
continue;
1167 std::vector<std::pair<double,double>> xy_pts;
1168 std::vector<std::pair<double,double>> rz_pts;
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)));
1176 if(
Verbosity()>0) std::cout <<
"chain size: " << chain.size() << std::endl;
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++)
1189 clean_chain.push_back(chain[i]);
1191 clean_chains.push_back(clean_chain);
1192 if(
Verbosity()>0) std::cout <<
"pushed clean chain with " << clean_chain.size() <<
" clusters" << std::endl;
1194 return clean_chains;
1201 for(
const auto&
seed:seeds )
1207 for(
const auto&
seed:seeds )