Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-08-06 08:18:23 

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