Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-12-17 09:20:55 

0001 #include "ALICEKF.h"
0002 
0003 #include "GPUTPCTrackLinearisation.h"
0004 #include "GPUTPCTrackParam.h"
0005 
0006 #include <trackbase/ClusterErrorPara.h>
0007 #include <trackbase/TrackFitUtils.h>
0008 #include <trackbase/TrkrCluster.h>
0009 #include <trackbase_historic/ActsTransformations.h>
0010 
0011 #include <Geant4/G4SystemOfUnits.hh>
0012 #include <cmath>
0013 
0014 #include <TMatrixFfwd.h>
0015 #include <TMatrixT.h>
0016 #include <TMatrixTUtils.h>
0017 
0018 #include <omp.h>
0019 
0020 //#define _DEBUG_
0021 
0022 #if defined(_DEBUG_)
0023 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp
0024 #else
0025 #define LogDebug(exp) (void) 0
0026 #endif
0027 
0028 #define LogError(exp) \
0029   if (Verbosity() > 0) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp
0030 #define LogWarning(exp) \
0031   if (Verbosity() > 0) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp
0032 
0033 using keylist = std::vector<TrkrDefs::cluskey>;
0034 
0035 // anonymous namespace for local functions
0036 namespace
0037 {
0038   // square
0039   template <class T>
0040   inline constexpr T square(const T& x)
0041   {
0042     return x * x;
0043   }
0044 }  // namespace
0045 
0046 bool ALICEKF::checknan(double val, const std::string& name, int num) const
0047 {
0048   if (std::isnan(val))
0049   {
0050     if (Verbosity() > 0)
0051     {
0052       std::cout << "WARNING: " << name << " is NaN for seed " << num << ". Aborting this seed.\n";
0053     }
0054   }
0055   return std::isnan(val);
0056 }
0057 
0058 
0059 ALICEKF::ALICEKF( TrkrClusterContainer* cmap, PHField* B, unsigned int min_clusters, double max_sin_phi, int verbosity):
0060   _B(B),
0061   _min_clusters_per_track(min_clusters),
0062   _cluster_map(cmap),
0063   _v(verbosity),
0064   _max_sin_phi(max_sin_phi),
0065   _ClusErrPara(new ClusterErrorPara)
0066 {}
0067 
0068 double ALICEKF::get_Bz(double x, double y, double z) const
0069 {
0070   // check z boundaries
0071   if (fabs(z) > 102.605)
0072   {
0073     // constant field is used when z is out of bound
0074     return _const_field;
0075   } else {
0076 
0077     // use field map
0078     const std::array<double,4> p = {x * cm, y * cm, z * cm, 0. * cm};
0079     double bfield[3];
0080 
0081     // check thread number. Use uncached field accessor for all but thread 0.
0082     if( omp_get_thread_num() == 0 )
0083     {
0084       _B->GetFieldValue(&p[0], bfield);
0085     } else {
0086       _B->GetFieldValue_nocache(&p[0], bfield);
0087     }
0088 
0089     return bfield[2] / tesla;
0090   }
0091 }
0092 
0093 double ALICEKF::getClusterError(TrkrCluster* c, TrkrDefs::cluskey key, Acts::Vector3 global, int i, int j) const
0094 {
0095   if (_use_fixed_clus_error)
0096   {
0097     if (i == j)
0098     {
0099       return _fixed_clus_error.at(i) * _fixed_clus_error.at(i);
0100     }
0101     else
0102     {
0103       return 0.;
0104     }
0105   }
0106   else
0107   {
0108     TMatrixF localErr(3, 3);
0109 
0110     double clusRadius = sqrt(global[0] * global[0] + global[1] * global[1]);
0111     auto para_errors = _ClusErrPara->get_clusterv5_modified_error(c, clusRadius, key);
0112 
0113     localErr[0][0] = 0.;
0114     localErr[0][1] = 0.;
0115     localErr[0][2] = 0.;
0116     localErr[1][0] = 0.;
0117     localErr[1][1] = para_errors.first;
0118     localErr[1][2] = 0.;
0119     localErr[2][0] = 0.;
0120     localErr[2][1] = 0.;
0121     localErr[2][2] = para_errors.second;
0122 
0123     float clusphi = atan2(global(1), global(0));
0124     TMatrixF ROT(3, 3);
0125     ROT[0][0] = std::cos(clusphi);
0126     ROT[0][1] = -std::sin(clusphi);
0127     ROT[0][2] = 0.0;
0128     ROT[1][0] = std::sin(clusphi);
0129     ROT[1][1] = std::cos(clusphi);
0130     ROT[1][2] = 0.0;
0131     ROT[2][0] = 0.0;
0132     ROT[2][1] = 0.0;
0133     ROT[2][2] = 1.0;
0134     TMatrixF ROT_T(3, 3);
0135     ROT_T.Transpose(ROT);
0136 
0137     TMatrixF err(3, 3);
0138     err = ROT * localErr * ROT_T;
0139 
0140     return err[i][j];
0141   }
0142 }
0143 
0144 bool ALICEKF::TransportAndRotate(double old_radius, double new_radius, double& phi, GPUTPCTrackParam& kftrack, GPUTPCTrackParam::GPUTPCTrackFitParam& fp) const
0145 {
0146   const double transport_spacing = .05;
0147   const int Ndivisions = floor(fabs(new_radius - old_radius) / transport_spacing);
0148   if (Verbosity() > 2)
0149   {
0150     std::cout << "old_radius: " << old_radius << ", new_radius: " << new_radius << std::endl;
0151   }
0152   if (Verbosity() > 2)
0153   {
0154     std::cout << "Ndivisions: " << Ndivisions << std::endl;
0155   }
0156   for (int i = 1; i <= Ndivisions + 1; i++)
0157   {
0158     if (std::isnan(kftrack.GetX()) ||
0159         std::isnan(kftrack.GetY()) ||
0160         std::isnan(kftrack.GetZ()))
0161     {
0162       if (Verbosity() > 0)
0163       {
0164         std::cout << "position is NaN, exiting" << std::endl;
0165       }
0166       return false;
0167     }
0168 
0169     if (new_radius > 78.)
0170     {
0171       if (Verbosity() > 1)
0172       {
0173         std::cout << "outside TPC, exiting" << std::endl;
0174       }
0175       return false;
0176     }
0177 
0178     double r_div;
0179     if (i == Ndivisions + 1)
0180     {
0181       r_div = new_radius;
0182     }
0183     else if (old_radius < new_radius)
0184     {
0185       r_div = old_radius + transport_spacing * i;
0186     }
0187     else
0188     {
0189       r_div = old_radius - transport_spacing * i;
0190     }
0191 
0192     // track state position relative to radial direction
0193     const double tX = kftrack.GetX();
0194     const double tY = kftrack.GetY();
0195     const double tz = kftrack.GetZ();
0196 
0197     // track state global position (including tz above)
0198     const double tx = tX * cos(phi) - tY * sin(phi);
0199     const double ty = tX * sin(phi) + tY * cos(phi);
0200 
0201     const double Bz = _Bzconst * get_Bz(tx, ty, tz);
0202 
0203     kftrack.CalculateFitParameters(fp);
0204 
0205     // transport to radius
0206     if (!kftrack.TransportToXWithMaterial(r_div, fp, Bz, 1.))
0207     {
0208       if (Verbosity() > 1)
0209       {
0210         std::cout << "transport failed" << std::endl;
0211       }
0212       return false;
0213     }
0214 
0215     // rotate track state reference frame
0216     const double new_tX = kftrack.GetX();
0217     const double new_tY = kftrack.GetY();
0218     const double new_tx = new_tX * cos(phi) - new_tY * sin(phi);
0219     const double new_ty = new_tX * sin(phi) + new_tY * cos(phi);
0220     const double new_phi = atan2(new_ty, new_tx);
0221     const double alpha = new_phi - phi;
0222     if (!kftrack.Rotate(alpha, 1.))
0223     {
0224       if (Verbosity() > 1)
0225       {
0226         std::cout << "rotate failed" << std::endl;
0227       }
0228       return false;
0229     }
0230     phi = new_phi;
0231   }
0232 
0233   const double final_tX = kftrack.GetX();
0234   const double final_tY = kftrack.GetY();
0235   const double final_tx = final_tX * cos(phi) - final_tY * sin(phi);
0236   const double final_ty = final_tX * sin(phi) + final_tY * cos(phi);
0237   const double final_tz = kftrack.GetZ();
0238 
0239   if (Verbosity() > 1)
0240   {
0241     std::cout << "track position after transport: (" << final_tx << ", " << final_ty << ", " << final_tz << ")" << std::endl;
0242   }
0243   return true;
0244 }
0245 
0246 bool ALICEKF::FilterStep(TrkrDefs::cluskey ckey, keylist& keys, double& current_phi, GPUTPCTrackParam& kftrack, GPUTPCTrackParam::GPUTPCTrackFitParam& fp, const PositionMap& globalPositions) const
0247 {
0248   // give up if position vector has NaN for any component
0249   if (std::isnan(kftrack.GetX()) ||
0250       std::isnan(kftrack.GetY()) ||
0251       std::isnan(kftrack.GetZ()))
0252   {
0253     if (Verbosity() > 0)
0254     {
0255       std::cout << "position is NaN, exiting" << std::endl;
0256     }
0257     return false;
0258   }
0259 
0260   // track state position relative to radial direction
0261   const double tX = kftrack.GetX();
0262   const double tY = kftrack.GetY();
0263   const double tz = kftrack.GetZ();
0264 
0265   // track state global position (including tz above)
0266   const double tx = tX * cos(current_phi) - tY * sin(current_phi);
0267   const double ty = tX * sin(current_phi) + tY * cos(current_phi);
0268 
0269   if (Verbosity() > 1)
0270   {
0271     std::cout << std::endl;
0272     std::cout << "current phi: " << current_phi << std::endl;
0273     std::cout << "track position: (" << tx << ", " << ty << ", " << tz << ")" << std::endl;
0274   }
0275 
0276   const auto& cluster_pos = globalPositions.at(ckey);
0277   const double cx = cluster_pos(0);
0278   const double cy = cluster_pos(1);
0279   const double cz = cluster_pos(2);
0280 
0281   if (Verbosity() > 1)
0282   {
0283     std::cout << "cluster position: (" << cx << ", " << cy << ", " << cz << ")" << std::endl;
0284   }
0285 
0286   const double cX = sqrt(cx * cx + cy * cy);
0287 
0288   if (Verbosity() > 1)
0289   {
0290     std::cout << "transporting to radius: " << cX << std::endl;
0291   }
0292 
0293   if (fabs(tX - cX) < 0.1 || !TransportAndRotate(tX, cX, current_phi, kftrack, fp))
0294   {
0295     if (std::isnan(kftrack.GetX()) ||
0296         std::isnan(kftrack.GetY()) ||
0297         std::isnan(kftrack.GetZ()))
0298     {
0299       return false;
0300     }
0301 
0302     if (Verbosity() > 1)
0303     {
0304       std::cout << "track turned around near X=" << kftrack.GetX() << std::endl;
0305     }
0306 
0307     // TransportAndRotate failure indicates that track has turned around before it reaches next layer
0308     // The track parameters don't update in that last step, so we're likely somewhere between two layers
0309     // So, first, we turn the track around ourselves, setting it to its next intersection at its current radius
0310 
0311     // basically circle project in xy, linear project in z
0312     double pt = 1. / fabs(kftrack.GetQPt());
0313     double end_tx = kftrack.GetX() * cos(current_phi) - kftrack.GetY() * sin(current_phi);
0314     double end_ty = kftrack.GetX() * sin(current_phi) + kftrack.GetY() * cos(current_phi);
0315     double end_tz = kftrack.GetZ();
0316     if (Verbosity() > 1)
0317     {
0318       std::cout << "current parameters: pt=" << pt << ", (x, y, z) = (" << end_tx << ", " << end_ty << ", " << end_tz << ")" << std::endl;
0319     }
0320     // pt[GeV] = 0.3 B[T] R[m]
0321     double R = 100. * pt / (0.3 * get_Bz(end_tx, end_ty, end_tz));
0322     if (Verbosity() > 2)
0323     {
0324       std::cout << "R=" << R << std::endl;
0325     }
0326     double pX = pt * kftrack.GetCosPhi();
0327     double pY = pt * kftrack.GetSinPhi();
0328     if (Verbosity() > 2)
0329     {
0330       std::cout << "(pX, pY) = (" << pX << ", " << pY << ")" << std::endl;
0331     }
0332     double px = pX * cos(current_phi) - pY * sin(current_phi);
0333     double py = pX * sin(current_phi) + pY * cos(current_phi);
0334     double tangent_phi = atan2(py, px);
0335     double center_phi;
0336     if (kftrack.GetQPt() > 0)
0337     {
0338       center_phi = tangent_phi + M_PI / 2.;
0339     }
0340     else
0341     {
0342       center_phi = tangent_phi - M_PI / 2.;
0343     }
0344     if (center_phi > M_PI)
0345     {
0346       center_phi -= 2. * M_PI;
0347     }
0348     if (center_phi < -M_PI)
0349     {
0350       center_phi += 2. * M_PI;
0351     }
0352     double xc = end_tx - R * cos(center_phi);
0353     double yc = end_ty - R * sin(center_phi);
0354     if (Verbosity() > 2)
0355     {
0356       std::cout << "(xc, yc) = (" << xc << ", " << yc << ")" << std::endl;
0357     }
0358     auto circle_output = TrackFitUtils::circle_circle_intersection(sqrt(pow(kftrack.GetX(), 2.) + pow(kftrack.GetY(), 2.)), R, xc, yc);
0359     // pick the furthest point from current track position
0360     double new_tx;
0361     double new_ty;
0362     double xplus = std::get<0>(circle_output);
0363     double yplus = std::get<1>(circle_output);
0364     double xminus = std::get<2>(circle_output);
0365     double yminus = std::get<3>(circle_output);
0366     if (Verbosity() > 1)
0367     {
0368       std::cout << "circle-circle intersection: (" << xplus << ", " << yplus << "), (" << xminus << ", " << yminus << ")" << std::endl;
0369     }
0370 
0371     if (sqrt(pow(end_tx - xplus, 2.) + pow(end_ty - yplus, 2.)) > sqrt(pow(end_tx - xminus, 2.) + pow(end_ty - yminus, 2.)))
0372     {
0373       new_tx = xplus;
0374       new_ty = yplus;
0375     }
0376     else
0377     {
0378       new_tx = xminus;
0379       new_ty = yminus;
0380     }
0381     double rot_phi = atan2(new_ty, new_tx);
0382     //    double rot_alpha = rot_phi - current_phi;
0383 
0384     // new track point is existing track point rotated by alpha
0385     double new_tX = new_tx * cos(rot_phi) + new_ty * sin(rot_phi);
0386     double new_tY = -new_tx * sin(rot_phi) + new_ty * cos(rot_phi);
0387     double new_centerphi = atan2(new_ty - yc, new_tx - xc);
0388     double dcenterphi = new_centerphi - center_phi;
0389     if (dcenterphi > M_PI)
0390     {
0391       dcenterphi = 2. * M_PI - dcenterphi;
0392     }
0393     if (dcenterphi < -M_PI)
0394     {
0395       dcenterphi = 2. * M_PI + dcenterphi;
0396     }
0397     double ds = R * fabs(dcenterphi);
0398     double dz = kftrack.GetDzDs() * ds;
0399 
0400     current_phi = rot_phi;
0401     kftrack.SetX(new_tX);
0402     kftrack.SetY(new_tY);
0403     kftrack.SetZ(end_tz + dz);
0404     // no change to sinPhi
0405     // no change to DzDs
0406     // no change to Q/pt
0407 
0408     kftrack.SetSignCosPhi(-kftrack.GetSignCosPhi());
0409 
0410     // now finish transport
0411     if (!TransportAndRotate(kftrack.GetX(), cX, current_phi, kftrack, fp))
0412     {
0413       return false;
0414     }
0415   }
0416 
0417   if (Verbosity() > 1)
0418   {
0419     const double new_tX = kftrack.GetX();
0420     const double new_tY = kftrack.GetY();
0421     const double new_tx = new_tX * cos(current_phi) - new_tY * sin(current_phi);
0422     const double new_ty = new_tX * sin(current_phi) + new_tY * cos(current_phi);
0423     const double new_tz = kftrack.GetZ();
0424     std::cout << "cluster position: (" << cx << ", " << cy << ", " << cz << ")" << std::endl;
0425     std::cout << "current phi: " << current_phi << std::endl;
0426     std::cout << "new track position: (" << new_tx << ", " << new_ty << ", " << new_tz << ")" << std::endl;
0427 
0428     const double tYerr = sqrt(kftrack.GetCov(0));
0429     const double txerr = fabs(tYerr * sin(current_phi));
0430     const double tyerr = fabs(tYerr * cos(current_phi));
0431     const double tzerr = sqrt(kftrack.GetCov(5));
0432     std::cout << "track position error: (" << txerr << ", " << tyerr << ", " << tzerr << ")" << std::endl;
0433   }
0434 
0435   TrkrCluster* cluster = _cluster_map->findCluster(ckey);
0436   const double cxerr = sqrt(getClusterError(cluster, ckey, cluster_pos, 0, 0));
0437   const double cyerr = sqrt(getClusterError(cluster, ckey, cluster_pos, 1, 1));
0438   const double czerr = sqrt(getClusterError(cluster, ckey, cluster_pos, 2, 2));
0439 
0440   if (Verbosity() > 1)
0441   {
0442     std::cout << "cluster position error: (" << cxerr << ", " << cyerr << ", " << czerr << ")" << std::endl;
0443   }
0444 
0445   const double cY = -cx * sin(current_phi) + cy * cos(current_phi);
0446   const double cxycov2 = getClusterError(cluster, ckey, cluster_pos, 0, 1);
0447   const double cYerr2 = cxerr * cxerr * sin(current_phi) * sin(current_phi) + cxycov2 * sin(current_phi) * cos(current_phi) + cyerr * cyerr * cos(current_phi) * cos(current_phi);
0448   const double czerr2 = czerr * czerr;
0449 
0450   if (Verbosity() > 1)
0451   {
0452     std::cout << "Filtering cluster with Y=" << cY << ", z=" << cz << ", Yerr2=" << cYerr2 << ", zerr2=" << czerr2 << std::endl;
0453   }
0454 
0455   bool filter_success = kftrack.Filter(cY, cz, cYerr2, czerr2, _max_sin_phi);
0456   if (Verbosity() > 1)
0457   {
0458     std::cout << "position after filter: (" << kftrack.GetX() * cos(current_phi) - kftrack.GetY() * sin(current_phi) << ", " << kftrack.GetX() * sin(current_phi) + kftrack.GetY() * cos(current_phi) << ", " << kftrack.GetZ() << std::endl;
0459     std::cout << "track current parameters:" << std::endl;
0460     std::cout << "(X, Y, Z) = (" << kftrack.GetX() << ", " << kftrack.GetY() << ", " << kftrack.GetZ() << ")" << std::endl;
0461     std::cout << "pt = " << 1. / fabs(kftrack.GetQPt()) << std::endl;
0462     std::cout << "QPt = " << kftrack.GetQPt() << std::endl;
0463     std::cout << "sinPhi = " << kftrack.GetSinPhi() << std::endl;
0464     std::cout << "cosPhi = " << kftrack.GetCosPhi() << std::endl;
0465     std::cout << "dzds = " << kftrack.GetDzDs() << std::endl;
0466   }
0467   if (!filter_success)
0468   {
0469     keys.erase(std::find(keys.begin(), keys.end(), ckey));
0470   }
0471   return true;
0472 }
0473 
0474 TrackSeedAliceSeedMap ALICEKF::ALICEKalmanFilter(const std::vector<keylist>& trackSeedKeyLists, bool use_nhits_limit, const PositionMap& globalPositions, std::vector<float>& trackChi2) const
0475 {
0476   //  TFile* f = new TFile("/sphenix/u/mjpeters/macros_hybrid/detectors/sPHENIX/pull.root", "RECREATE");
0477   //  TNtuple* ntp = new TNtuple("pull","pull","cx:cy:cz:xerr:yerr:zerr:tx:ty:tz:layer:xsize:ysize:phisize:phierr:zsize");
0478   std::vector<TrackSeed_v2> seeds_vector;
0479   std::vector<GPUTPCTrackParam> alice_seeds_vector;
0480   int nseeds = 0;
0481   int ncandidates = -1;
0482   if (Verbosity() > 0)
0483   {
0484     std::cout << "min clusters per track: " << _min_clusters_per_track << "\n";
0485   }
0486   for (auto trackKeyChain : trackSeedKeyLists)
0487   {
0488     ++ncandidates;
0489 
0490     if (trackKeyChain.size() < 2)
0491     {
0492       continue;
0493     }
0494     if (use_nhits_limit && trackKeyChain.size() < _min_clusters_per_track)
0495     {
0496       continue;
0497     }
0498     //    if(TrkrDefs::getLayer(trackKeyChain.front())<TrkrDefs::getLayer(trackKeyChain.back())) { std::reverse(trackKeyChain.begin(),trackKeyChain.end()); }
0499     // get starting cluster from key
0500     // Transform sPHENIX coordinates into ALICE-compatible coordinates
0501     const auto& globalpos = globalPositions.at(trackKeyChain.at(0));
0502     double x0 = globalpos(0);
0503     double y0 = globalpos(1);
0504     double z0 = globalpos(2);
0505     ;
0506     LogDebug("Initial (x,y,z): (" << x0 << "," << y0 << "," << z0 << ")" << std::endl);
0507     // ALICE x coordinate = distance from beampipe
0508     double alice_x0 = sqrt(x0 * x0 + y0 * y0);
0509     double alice_y0 = 0;
0510     double alice_z0 = z0;
0511     // Initialize track and linearisation
0512     GPUTPCTrackParam trackSeed;
0513     trackSeed.setNeonFraction(Ne_frac);
0514     trackSeed.setArgonFraction(Ar_frac);
0515     trackSeed.setCF4Fraction(CF4_frac);
0516     trackSeed.setNitrogenFraction(N2_frac);
0517     trackSeed.setIsobutaneFraction(isobutane_frac);
0518     trackSeed.InitParam();
0519     trackSeed.SetX(alice_x0);
0520     trackSeed.SetY(alice_y0);
0521     trackSeed.SetZ(alice_z0);
0522     double x = x0;
0523     double y = y0;
0524 #if defined(_DEBUG_)
0525     double z = z0;
0526     double alice_x = sqrt(x0 * x0 + y0 * y0);
0527 #endif
0528     // Pre-set momentum-based parameters to improve numerical stability
0529     const auto& secondpos = globalPositions.at(trackKeyChain.at(1));
0530 
0531     const double second_x = secondpos(0);
0532     const double second_y = secondpos(1);
0533     const double second_z = secondpos(2);
0534     const double first_phi = atan2(y0, x0);
0535     const double second_alice_x = second_x * std::cos(first_phi) + second_y * std::sin(first_phi);
0536     const double delta_alice_x = second_alice_x - alice_x0;
0537     // 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));
0538     const double second_alice_y = -second_x * std::sin(first_phi) + second_y * std::cos(first_phi);
0539     double init_SinPhi = second_alice_y / std::sqrt(square(delta_alice_x) + square(second_alice_y));
0540     const double delta_z = second_z - z0;
0541     double init_DzDs = delta_z / std::sqrt(square(delta_alice_x) + square(second_alice_y));
0542     if (delta_alice_x < 0.)
0543     {
0544       init_SinPhi *= -1.;
0545       init_DzDs *= -1.;
0546     }
0547     trackSeed.SetSinPhi(init_SinPhi);
0548     // trackSeed.SetSignCosPhi(delta_alice_x / std::sqrt(square(delta_alice_x) + square(second_alice_y)));
0549     LogDebug("Set initial SinPhi to " << init_SinPhi << std::endl);
0550     trackSeed.SetDzDs(init_DzDs);
0551     LogDebug("Set initial DzDs to " << init_DzDs << std::endl);
0552 
0553     // get initial pt estimate
0554     std::vector<std::pair<double, double>> pts;
0555     std::transform(trackKeyChain.begin(), trackKeyChain.end(), std::back_inserter(pts), [&globalPositions](const TrkrDefs::cluskey& key)
0556                    {
0557       const auto& clpos = globalPositions.at(key);
0558       return std::make_pair(clpos(0),clpos(1)); });
0559 
0560     const auto [R, x_center, y_center] = TrackFitUtils::circle_fit_by_taubin(pts);
0561     if (Verbosity() > 1)
0562     {
0563       std::cout << std::endl
0564                 << "candidate " << ncandidates << " seed " << nseeds << " circle fit parameters: R=" << R << ", X0=" << x_center << ", Y0=" << y_center << std::endl;
0565     }
0566 
0567     // check circle fit success
0568     /* failed fit will result in infinite momentum for the track, which in turn will break the kalman filter */
0569     if (std::isnan(R))
0570     {
0571       continue;
0572     }
0573 
0574     double init_QPt = 1. / (0.3 * R / 100. * get_Bz(x0, y0, z0));
0575     // determine charge
0576     double phi_first = atan2(y0, x0);
0577     if (Verbosity() > 1)
0578     {
0579       std::cout << "phi_first: " << phi_first << std::endl;
0580     }
0581     double phi_second = atan2(second_y, second_x);
0582     if (Verbosity() > 1)
0583     {
0584       std::cout << "phi_second: " << phi_second << std::endl;
0585     }
0586     double dphi = phi_second - phi_first;
0587     if (Verbosity() > 1)
0588     {
0589       std::cout << "dphi: " << dphi << std::endl;
0590     }
0591     if (dphi > M_PI)
0592     {
0593       dphi = 2 * M_PI - dphi;
0594     }
0595     if (dphi < -M_PI)
0596     {
0597       dphi = 2 * M_PI + dphi;
0598     }
0599     if (Verbosity() > 1)
0600     {
0601       std::cout << "corrected dphi: " << dphi << std::endl;
0602     }
0603     if ((dphi > 0. && x0 * x0 + y0 * y0 < second_x * second_x + second_y * second_y) ||
0604         (dphi < 0. && x0 * x0 + y0 * y0 > second_x * second_x + second_y * second_y))
0605     {
0606       init_QPt = -1 * init_QPt;
0607     }
0608     LogDebug("initial QPt: " << init_QPt << std::endl);
0609     trackSeed.SetQPt(init_QPt);
0610     /*
0611         if (trackSeed.GetSignCosPhi() < 0) {
0612           trackSeed.SetSignCosPhi(-trackSeed.GetSignCosPhi());
0613           trackSeed.SetSinPhi(-trackSeed.GetSinPhi());
0614           trackSeed.SetDzDs(-trackSeed.GetDzDs());
0615           trackSeed.SetQPt(-trackSeed.GetQPt());
0616           trackSeed.SetCov(3,-trackSeed.GetCov(3));
0617           trackSeed.SetCov(4,-trackSeed.GetCov(4));
0618           trackSeed.SetCov(6,-trackSeed.GetCov(6));
0619           trackSeed.SetCov(7,-trackSeed.GetCov(7));
0620           trackSeed.SetCov(10,-trackSeed.GetCov(10));
0621           trackSeed.SetCov(11,-trackSeed.GetCov(11));
0622         }
0623     */
0624     GPUTPCTrackLinearisation trackLine(trackSeed);
0625     GPUTPCTrackParam::GPUTPCTrackFitParam fp{};
0626     trackSeed.CalculateFitParameters(fp);
0627 
0628     LogDebug(std::endl
0629              << std::endl
0630              << "------------------------" << std::endl
0631              << "seed size: " << trackKeyChain.size() << std::endl
0632              << std::endl
0633              << std::endl);
0634     double current_phi = phi_first;
0635     bool filter_failed = false;
0636     keylist outputKeyChain = trackKeyChain;
0637     for (auto clusterkey = std::next(trackKeyChain.begin()); clusterkey != trackKeyChain.end(); ++clusterkey)
0638     {
0639       if (!FilterStep(*clusterkey, outputKeyChain, current_phi, trackSeed, fp, globalPositions))
0640       {
0641         if (Verbosity() > 0)
0642         {
0643           std::cout << "Kalman filter failed, exiting" << std::endl;
0644         }
0645         filter_failed = true;
0646         break;
0647       }
0648     }
0649     if (filter_failed)
0650     {
0651       continue;
0652     }
0653     if (Verbosity() > 0)
0654     {
0655       std::cout << "finished track\n";
0656     }
0657 
0658     double track_phi = atan2(y, x);
0659 
0660     if (Verbosity() > 1)
0661     {
0662       std::cout << "final QPt = " << trackSeed.GetQPt() << std::endl;
0663     }
0664 
0665     double track_pt = fabs(1. / trackSeed.GetQPt());
0666 #if defined(_DEBUG_)
0667     double track_pY = track_pt * trackSeed.GetSinPhi();
0668     double track_pX = sqrt(track_pt * track_pt - track_pY * track_pY);
0669     double track_px = track_pX * cos(track_phi) - track_pY * sin(track_phi);
0670     double track_py = track_pX * sin(track_phi) + track_pY * cos(track_phi);
0671     double track_pz = track_pt * trackSeed.GetDzDs();
0672 #endif
0673     double track_pterr = sqrt(trackSeed.GetErr2QPt()) / (trackSeed.GetQPt() * trackSeed.GetQPt());
0674     // If Kalman filter doesn't do its job (happens often with short seeds), use the circle-fit estimate as the central value
0675     // if(trackKeyChain.size()<10) {track_pt = fabs(1./init_QPt);}
0676     LogDebug("track pt = " << track_pt << " +- " << track_pterr << std::endl);
0677     LogDebug("track ALICE p = (" << track_pX << ", " << track_pY << ", " << track_pz << ")" << std::endl);
0678     LogDebug("track p = (" << track_px << ", " << track_py << ", " << track_pz << ")" << std::endl);
0679 
0680     /*
0681         if(cluster_ctr!=1 && !trackSeed.CheckNumericalQuality())
0682         {
0683           std::cout << "ERROR: Track seed failed numerical quality check before conversion to sPHENIX coordinates! Skipping this one.\n";
0684           aborted = true;
0685           continue;
0686         }
0687     */
0688     //    pt:z:dz:phi:dphi:c:dc
0689     // Fill NT with track parameters
0690     // double StartEta = -log(tan(atan(z0/sqrt(x0*x0+y0*y0))));
0691     //    if(aborted) continue;
0692     //    double track_pt = fabs( 1./(trackSeed.GetQPt()));
0693     if (checknan(track_pt, "pT", nseeds))
0694     {
0695       continue;
0696     }
0697     //    double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
0698     if (checknan(track_pterr, "pT err", nseeds))
0699     {
0700       continue;
0701     }
0702     LogDebug("Track pterr = " << track_pterr << std::endl);
0703     double track_x = trackSeed.GetX() * cos(track_phi) - trackSeed.GetY() * sin(track_phi);
0704     double track_y = trackSeed.GetX() * sin(track_phi) + trackSeed.GetY() * cos(track_phi);
0705     double track_z = trackSeed.GetZ();
0706     if (checknan(track_z, "z", nseeds))
0707     {
0708       continue;
0709     }
0710     double track_zerr = sqrt(trackSeed.GetErr2Z());
0711     if (checknan(track_zerr, "zerr", nseeds))
0712     {
0713       continue;
0714     }
0715     auto lcluster = _cluster_map->findCluster(trackKeyChain.back());
0716     const auto& lclusterglob = globalPositions.at(trackKeyChain.back());
0717     const float lclusterrad = sqrt(lclusterglob(0) * lclusterglob(0) + lclusterglob(1) * lclusterglob(1));
0718     double last_cluster_phierr = lcluster->getRPhiError() / lclusterrad;
0719     ;
0720 
0721     // phi error assuming error in track radial coordinate is zero
0722     double track_phierr = sqrt(pow(last_cluster_phierr, 2) + (pow(trackSeed.GetX(), 2) * trackSeed.GetErr2Y()) /
0723                                                                  pow(pow(trackSeed.GetX(), 2) + pow(trackSeed.GetY(), 2), 2));
0724     if (checknan(track_phierr, "phierr", nseeds))
0725     {
0726       continue;
0727     }
0728     LogDebug("Track phi = " << atan2(track_py, track_px) << std::endl);
0729     LogDebug("Track phierr = " << track_phierr << std::endl);
0730     double track_curvature = trackSeed.GetKappa(_Bzconst * get_Bz(track_x, track_y, track_z));
0731     if (checknan(track_curvature, "curvature", nseeds))
0732     {
0733       continue;
0734     }
0735     double track_curverr = sqrt(trackSeed.GetErr2QPt()) * _Bzconst * get_Bz(track_x, track_y, track_z);
0736     if (checknan(track_curverr, "curvature error", nseeds))
0737     {
0738       continue;
0739     }
0740     TrackSeed_v2 track;
0741     //    track.set_vertex_id(_vertex_ids[best_vtx]);
0742     for (unsigned long j : outputKeyChain)
0743     {
0744       track.insert_cluster_key(j);
0745     }
0746 
0747     int track_charge = 0;
0748     if (trackSeed.GetQPt() < 0)
0749     {
0750       track_charge = -1 * trackSeed.GetSignCosPhi();
0751     }
0752     else
0753     {
0754       track_charge = 1 * trackSeed.GetSignCosPhi();
0755     }
0756 
0757     double sinphi = sin(track_phi);  // who had the idea to use s here?????
0758     double c = cos(track_phi);
0759     double p = trackSeed.GetSinPhi();
0760 
0761     /// Shows the transformation between ALICE and sPHENIX coordinates
0762     // track.set_x(trackSeed.GetX()*c-trackSeed.GetY()*s);//_vertex_x[best_vtx]);  //track.set_x(cl->getX());
0763     // track.set_y(trackSeed.GetX()*s+trackSeed.GetY()*c);//_vertex_y[best_vtx]);  //track.set_y(cl->getY());
0764     // track.set_z(trackSeed.GetZ());//_vertex_z[best_vtx]);  //track.set_z(cl->getZ());
0765     // if(Verbosity()>0) std::cout << "x " << track.get_x() << "\n";
0766     // if(Verbosity()>0) std::cout << "y " << track.get_y() << "\n";
0767     // if(Verbosity()>0) std::cout << "z " << track.get_z() << "\n";
0768     // if(checknan(p,"ALICE sinPhi",nseeds)) continue;
0769     double d = trackSeed.GetDzDs();
0770     if (checknan(d, "ALICE dz/ds", nseeds))
0771     {
0772       continue;
0773     }
0774 
0775     /// Shows the transformation between ALICE and sPHENIX coordinates
0776     // double pY = track_pt*p;
0777     // double pX = sqrt(track_pt*track_pt-pY*pY);
0778     /// We set the qoverR to get the good charge estimate from the KF
0779     /// which helps the Acts fit
0780     track.set_qOverR(trackSeed.GetQPt() * (0.3 * _const_field) / 100.);
0781     // track.set_px(pX*c-pY*s);
0782     // track.set_py(pX*s+pY*c);
0783     // track.set_pz(track_pt * trackSeed.GetDzDs());
0784     const double* cov = trackSeed.GetCov();
0785     bool cov_nan = false;
0786     for (int i = 0; i < 15; i++)
0787     {
0788       if (checknan(cov[i], "covariance element " + std::to_string(i), nseeds))
0789       {
0790         cov_nan = true;
0791       }
0792     }
0793     if (cov_nan)
0794     {
0795       continue;
0796     }
0797     // make this into an actual Eigen matrix
0798     Eigen::Matrix<double, 5, 5> ecov;
0799     ecov(0, 0) = cov[0];
0800     ecov(0, 1) = cov[1];
0801     ecov(0, 2) = cov[2];
0802     ecov(0, 3) = cov[3];
0803     ecov(0, 4) = cov[4];
0804     ecov(1, 1) = cov[5];
0805     ecov(1, 2) = cov[6];
0806     ecov(1, 3) = cov[7];
0807     ecov(1, 4) = cov[8];
0808     ecov(2, 2) = cov[9];
0809     ecov(2, 3) = cov[10];
0810     ecov(2, 4) = cov[11];
0811     ecov(3, 3) = cov[12];
0812     ecov(3, 4) = cov[13];
0813     ecov(4, 4) = cov[14];
0814     // symmetrize
0815     ecov(1, 0) = ecov(0, 1);
0816     ecov(2, 0) = ecov(0, 2);
0817     ecov(3, 0) = ecov(0, 3);
0818     ecov(4, 0) = ecov(0, 4);
0819     ecov(2, 1) = ecov(1, 2);
0820     ecov(3, 1) = ecov(1, 3);
0821     ecov(4, 1) = ecov(1, 4);
0822     ecov(3, 2) = ecov(2, 3);
0823     ecov(4, 2) = ecov(2, 4);
0824     ecov(4, 3) = ecov(3, 4);
0825     // make rotation matrix based on the following:
0826     // x = X*cos(track_phi) - Y*sin(track_phi)
0827     // y = X*sin(track_phi) + Y*cos(track_phi)
0828     // z = Z
0829     // pY = pt*sinphi
0830     // pX = sqrt(pt**2 - pY**2)
0831     // px = pX*cos(track_phi) - pY*sin(track_phi)
0832     // py = pX*sin(track_phi) + pY*cos(track_phi)
0833     // pz = pt*(dz/ds)
0834     Eigen::Matrix<double, 6, 5> J;
0835     J(0, 0) = -sinphi;  // dx/dY
0836     J(0, 1) = 0.;       // dx/dZ
0837     J(0, 2) = 0.;       // dx/d(sinphi)
0838     J(0, 3) = 0.;       // dx/d(dz/ds)
0839     J(0, 4) = 0.;       // dx/d(Q/pt)
0840 
0841     J(1, 0) = c;   // dy/dY
0842     J(1, 1) = 0.;  // dy/dZ
0843     J(1, 2) = 0.;  // dy/d(sinphi)
0844     J(1, 3) = 0.;  // dy/d(dz/ds)
0845     J(1, 4) = 0.;  // dy/d(Q/pt)
0846 
0847     J(2, 0) = 0.;  // dz/dY
0848     J(2, 1) = 1.;  // dz/dZ
0849     J(2, 2) = 0.;  // dz/d(sinphi)
0850     J(2, 3) = 0.;  // dz/d(dz/ds)
0851     J(2, 4) = 0.;  // dz/d(Q/pt)
0852 
0853     J(3, 0) = 0.;                                                                       // dpx/dY
0854     J(3, 1) = 0.;                                                                       // dpx/dZ
0855     J(3, 2) = -track_pt * (p * c / sqrt(1 - p * p) + sinphi);                           // dpx/d(sinphi)
0856     J(3, 3) = 0.;                                                                       // dpx/d(dz/ds)
0857     J(3, 4) = track_pt * track_pt * track_charge * (p * sinphi - c * sqrt(1 - p * p));  // dpx/d(Q/pt)
0858 
0859     J(4, 0) = 0.;                                                                        // dpy/dY
0860     J(4, 1) = 0.;                                                                        // dpy/dZ
0861     J(4, 2) = track_pt * (c - p * sinphi / sqrt(1 - p * p));                             // dpy/d(sinphi)
0862     J(4, 3) = 0.;                                                                        // dpy/d(dz/ds)
0863     J(4, 4) = -track_pt * track_pt * track_charge * (p * c + sinphi * sqrt(1 - p * p));  // dpy/d(Q/pt)
0864 
0865     J(5, 0) = 0.;                                       // dpz/dY
0866     J(5, 1) = 0.;                                       // dpz/dZ
0867     J(5, 2) = 0.;                                       // dpz/d(sinphi)
0868     J(5, 3) = track_pt;                                 // dpz/d(dz/ds)
0869     J(5, 4) = -track_pt * track_pt * track_charge * d;  // dpz/d(Q/pt)
0870                                                         /*    bool cov_rot_nan = false;
0871                                                             for(int i=0;i<6;i++)
0872                                                             {
0873                                                               for(int j=0;j<5;j++)
0874                                                               {
0875                                                                 if(checknan(J(i,j),"covariance rotator element ("+std::to_string(i)+","+std::to_string(j)+")",nseeds))
0876                                                                 {
0877                                                                   cov_rot_nan = true;
0878                                                                   continue;
0879                                                                 }
0880                                                               }
0881                                                             }
0882                                                             if(cov_rot_nan) continue;
0883                                                         */
0884     // the heavy lifting happens here
0885     Eigen::Matrix<double, 6, 6> scov = J * ecov * J.transpose();
0886     if (!covIsPosDef(scov))
0887     {
0888       repairCovariance(scov);
0889     }
0890     /*
0891     // Derived from:
0892     // 1) Taking the Jacobian of the conversion from (Y,Z,SinPhi,DzDs,Q/Pt) to (x,y,z,px,py,pz)
0893     // 2) Computing (Jacobian)*(ALICE covariance matrix)*(transpose of Jacobian)
0894     track.set_error(0, 0, cov[0]*s*s);
0895     track.set_error(0, 1, -cov[0]*c*s);
0896     track.set_error(0, 2, -cov[1]*s);
0897     track.set_error(0, 3, cov[2]*s*s/q-cov[4]*s*(-c/(q*q)+p*s/(q*q)));
0898     track.set_error(0, 4, -cov[2]*c*s/q-cov[4]*s*(-c*p/(q*q)-s/(q*q)));
0899     track.set_error(0, 5, cov[4]*d*s/(q*q)-cov[3]*s/q);
0900     track.set_error(1, 1, cov[0]*c*c);
0901     track.set_error(1, 2, cov[1]*c);
0902     track.set_error(1, 3, -cov[2]*c*s/q+cov[4]*c*(-c/(q*q)+p*s/(q*q)));
0903     track.set_error(1, 4, cov[2]*c*c/q+cov[4]*c*(-c*p/(q*q)-s/(q*q)));
0904     track.set_error(1, 5, cov[4]*d*c/(q*q)+cov[3]*c/q);
0905     track.set_error(2, 2, cov[5]);
0906     track.set_error(2, 3, -cov[6]*s/q+cov[8]*(-c/(q*q)+p*s/(q*q)));
0907     track.set_error(2, 4, cov[6]*c/q+cov[8]*(-c*p/(q*q)-s/(q*q)));
0908     track.set_error(2, 5, -cov[8]*d/(q*q)+cov[7]/q);
0909     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))));
0910     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))));
0911     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))));
0912     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))));
0913     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))));
0914     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));
0915     // symmetrize covariance
0916     track.set_error(1, 0, track.get_error(0, 1));
0917     track.set_error(2, 0, track.get_error(0, 2));
0918     track.set_error(3, 0, track.get_error(0, 3));
0919     track.set_error(4, 0, track.get_error(0, 4));
0920     track.set_error(5, 0, track.get_error(0, 5));
0921     track.set_error(2, 1, track.get_error(1, 2));
0922     track.set_error(3, 1, track.get_error(1, 3));
0923     track.set_error(4, 1, track.get_error(1, 4));
0924     track.set_error(5, 1, track.get_error(1, 5));
0925     track.set_error(3, 2, track.get_error(2, 3));
0926     track.set_error(4, 2, track.get_error(2, 4));
0927     track.set_error(5, 2, track.get_error(2, 5));
0928     track.set_error(4, 3, track.get_error(3, 4));
0929     track.set_error(5, 3, track.get_error(3, 5));
0930     track.set_error(5, 4, track.get_error(4, 5));
0931 */
0932 
0933     /*
0934         for(int w=0;w<cx.size();w++)
0935         {
0936           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]);
0937         }
0938         cx.clear();
0939         cy.clear();
0940         cz.clear();
0941         tx.clear();
0942         ty.clear();
0943         tz.clear();
0944         xerr.clear();
0945         yerr.clear();
0946         zerr.clear();
0947         layer.clear();
0948         xsize.clear();
0949         ysize.clear();
0950         phisize.clear();
0951         phierr.clear();
0952         zsize.clear();
0953     */
0954     seeds_vector.push_back(track);
0955     alice_seeds_vector.push_back(trackSeed);
0956     trackChi2.push_back(trackSeed.GetChi2() / trackSeed.GetNDF());
0957 
0958     ++nseeds;
0959   }
0960   //  f->cd();
0961   //  ntp->Write();
0962   //  f->Close();
0963   if (Verbosity() > 0)
0964   {
0965     std::cout << "number of seeds: " << nseeds << "\n";
0966   }
0967 
0968   return std::make_pair(seeds_vector, alice_seeds_vector);
0969 }
0970 
0971 bool ALICEKF::covIsPosDef(Eigen::Matrix<double, 6, 6>& cov) const
0972 {
0973   // attempt Cholesky decomposition
0974   Eigen::LLT<Eigen::Matrix<double, 6, 6>> chDec(cov);
0975   // if Cholesky decomposition does not exist, matrix is not positive definite
0976   return (chDec.info() != Eigen::NumericalIssue);
0977 }
0978 
0979 void ALICEKF::repairCovariance(Eigen::Matrix<double, 6, 6>& cov) const
0980 {
0981   Eigen::Matrix<double, 6, 6> repaircov = cov;
0982   // find closest positive definite matrix
0983   Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> solver(repaircov);
0984   const Eigen::Matrix<double, 6, 1>& D = solver.eigenvalues();
0985   Eigen::Matrix<double, 6, 6> Q = solver.eigenvectors();
0986   Eigen::Matrix<double, 6, 1> Dp = D.cwiseMax(1e-15);
0987   Eigen::Matrix<double, 6, 6> Z = Q * Dp.asDiagonal() * Q.transpose();
0988   // updates covariance matrix
0989   for (int i = 0; i < 6; i++)
0990   {
0991     for (int j = 0; j < 6; j++)
0992     {
0993       cov(i, j) = Z(i, j);
0994     }
0995   }
0996 }
0997 
0998 std::vector<double> ALICEKF::GetCircleClusterResiduals(const std::vector<std::pair<double, double>>& points, double R, double X0, double Y0) const
0999 {
1000   std::vector<double> residues;
1001   std::transform(points.begin(), points.end(), std::back_inserter(residues), [R, X0, Y0](const std::pair<double, double>& point)
1002                  {
1003     double x = point.first;
1004     double y = point.second;
1005 
1006     // The shortest distance of a point from a circle is along the radial; line from the circle center to the point
1007     return std::sqrt( square(x-X0) + square(y-Y0) )  -  R; });
1008   return residues;
1009 }
1010 
1011 std::vector<double> ALICEKF::GetLineClusterResiduals(const std::vector<std::pair<double, double>>& points, double A, double B) const
1012 {
1013   std::vector<double> residues;
1014   // calculate cluster residuals from the fitted circle
1015   std::transform(points.begin(), points.end(), std::back_inserter(residues), [A, B](const std::pair<double, double>& point)
1016                  {
1017     double r = point.first;
1018     double z = point.second;
1019 
1020     // The shortest distance of a point from a circle is along the radial; line from the circle center to the point
1021 
1022     double a = -A;
1023     double b = 1.0;
1024     double c = -B;
1025     return std::abs(a*r+b*z+c)/sqrt(square(a)+square(b)); });
1026   return residues;
1027 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!