Fork me on GitHub

Ignore:
Timestamp:
Jul 9, 2020, 3:14:29 PM (4 years ago)
Author:
Michele Selvaggi <michele.selvaggi@…>
Branches:
master
Children:
c18dca6
Parents:
e79c954
Message:

updated to ACTS compliant ObsTrk class

File:
1 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/ObsTrk.cc

    re79c954 r942a705  
    1 #include <iostream>
    2 
    31#include <TMath.h>
    42#include <TVector3.h>
    … …  
    75#include <TDecompChol.h>
    86#include <TRandom.h>
    9 
     7#include <iostream>
    108#include "SolGridCov.h"
    119#include "ObsTrk.h"
    12 
    13 using namespace std;
    14 
     10//
     11// Constructors
    1512// x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
    1613ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC)
    1714{
    18   fGC = GC;
    19   fGenX = x;
    20   fGenP = p;
    21   fGenQ = Q;
    22   fB = B;
    23   fGenPar.ResizeTo(5);
    24   fObsPar.ResizeTo(5);
    25   fCov.ResizeTo(5, 5);
    26   fGenPar = XPtoPar(x, p, Q);
    27   fObsPar = GenToObsPar(fGenPar, fGC);
    28   fObsX = ParToX(fObsPar);
    29   fObsP = ParToP(fObsPar);
    30   fObsQ = ParToQ(fObsPar);
    31 }
    32 
     15        fGC = GC;
     16        fGenX = x;
     17        fGenP = p;
     18        fGenQ = Q;
     19        fB = B;
     20        fGenPar.ResizeTo(5);
     21        fGenParACTS.ResizeTo(6);
     22        fObsPar.ResizeTo(5);
     23        fObsParACTS.ResizeTo(6);
     24        fCov.ResizeTo(5, 5);
     25        fCovACTS.ResizeTo(6, 6);
     26        fGenPar = XPtoPar(x,p,Q);
     27        fGenParACTS = ParToACTS(fGenPar);
     28        /*
     29        std::cout << "ObsTrk::ObsTrk: fGenPar";
     30        for (Int_t i = 0; i < 5; i++)std::cout << fGenPar(i) << ", ";
     31        std::cout << std::endl;
     32        */
     33        fObsPar = GenToObsPar(fGenPar, fGC);
     34        fObsParACTS = ParToACTS(fObsPar);
     35        fObsX = ParToX(fObsPar);
     36        fObsP = ParToP(fObsPar);
     37        fObsQ = ParToQ(fObsPar);
     38        fCovACTS = CovToACTS(fCov);
     39}
     40//
     41// Destructor
    3342ObsTrk::~ObsTrk()
    3443{
    35 }
    36 
     44        fGenX.Clear();
     45        fGenP.Clear();
     46        fGenPar.Clear();
     47        fGenParACTS.Clear();
     48        fObsX.Clear();
     49        fObsP.Clear();
     50        fObsPar.Clear();
     51        fObsParACTS.Clear();
     52        fCov.Clear();
     53        fCovACTS.Clear();
     54}
    3755TVectorD ObsTrk::XPtoPar(TVector3 x, TVector3 p, Double_t Q)
    3856{
    39   TVectorD Par(5);
    40   // Transverse parameters
    41   Double_t a = -Q * fB * 0.2998; // Units are Tesla, GeV and m
    42   Double_t pt = p.Pt();
    43   Double_t C = a / (2 * pt); // Half curvature
    44 
    45   Double_t r2 = x.Perp2();
    46   Double_t cross = x(0) * p(1) - x(1) * p(0);
    47   Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
    48   Double_t phi0 = TMath::ATan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T); // Phi0
    49   Double_t D; // Impact parameter D
    50   if (pt < 10.0) D = (T - pt) / a;
    51   else D = (-2 * cross + a * r2) / (T + pt);
    52 
    53   Par(0) = D; // Store D
    54   Par(1) = phi0; // Store phi0
    55   Par(2) = C; // Store C
    56   // Longitudinal parameters
    57   Double_t B = C * TMath::Sqrt(TMath::Max(r2 - D * D,0.0) / (1 + 2 * C * D));
    58   Double_t st = TMath::ASin(B) / C;
    59   Double_t ct = p(2) / pt;
    60   Double_t z0 = x(2) - ct * st;
    61 
    62   Par(3) = z0; // Store z0
    63   Par(4) = ct; // Store cot(theta)
    64 
    65   return Par;
    66 }
    67 
     57        //
     58        TVectorD Par(5);
     59        // Transverse parameters
     60        Double_t a = -Q*fB*0.2998;                      // Units are Tesla, GeV and meters
     61        Double_t pt = p.Pt();
     62        Double_t C = a / (2 * pt);                      // Half curvature
     63        //std::cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << std::endl;
     64        Double_t r2 = x.Perp2();
     65        Double_t cross = x(0)*p(1) - x(1)*p(0);
     66        Double_t T = TMath::Sqrt(pt*pt - 2 * a*cross + a*a*r2);
     67        Double_t phi0 = TMath::ATan2((p(1) - a*x(0)) / T, (p(0) + a*x(1)) / T); // Phi0
     68        Double_t D;                                                     // Impact parameter D
     69        if (pt < 10.0) D = (T - pt) / a;
     70        else D = (-2 * cross + a*r2) / (T + pt);
     71        //
     72        Par(0) = D;             // Store D
     73        Par(1) = phi0;  // Store phi0
     74        Par(2) = C;             // Store C
     75        //Longitudinal parameters
     76        Double_t B = C*TMath::Sqrt(TMath::Max(r2 - D*D,0.0) / (1 + 2 * C*D));
     77        Double_t st = TMath::ASin(B) / C;
     78        Double_t ct = p(2) / pt;
     79        Double_t z0 = x(2) - ct*st;
     80        //
     81        Par(3) = z0;            // Store z0
     82        Par(4) = ct;            // Store cot(theta)
     83        //
     84        return Par;
     85}
     86//
    6887TVector3 ObsTrk::ParToX(TVectorD Par)
    6988{
    70   Double_t D    = Par(0);
    71   Double_t phi0 = Par(1);
    72   Double_t z0   = Par(3);
    73 
    74   TVector3 Xval;
    75   Xval(0) = -D * TMath::Sin(phi0);
    76   Xval(1) =  D * TMath::Cos(phi0);
    77   Xval(2) =  z0;
    78 
    79   return Xval;
    80 }
    81 
     89        Double_t D    = Par(0);
     90        Double_t phi0 = Par(1);
     91        Double_t z0   = Par(3);
     92        //
     93        TVector3 Xval;
     94        Xval(0) = -D*TMath::Sin(phi0);
     95        Xval(1) =  D*TMath::Cos(phi0); 
     96        Xval(2) =  z0;
     97        //
     98        return Xval;
     99}
     100//
    82101TVector3 ObsTrk::ParToP(TVectorD Par)
    83102{
    84   Double_t C    = Par(2);
    85   Double_t phi0 = Par(1);
    86   Double_t ct   = Par(4);
    87   //
    88   TVector3 Pval;
    89   Double_t pt = fB * 0.2998 / TMath::Abs(2 * C);
    90   Pval(0) = pt * TMath::Cos(phi0);
    91   Pval(1) = pt * TMath::Sin(phi0);
    92   Pval(2) = pt * ct;
    93   //
    94   return Pval;
    95 }
     103        Double_t C    = Par(2);
     104        Double_t phi0 = Par(1);
     105        Double_t ct   = Par(4);
     106        //
     107        TVector3 Pval;
     108        Double_t pt = fB*0.2998 / TMath::Abs(2 * C);
     109        Pval(0) = pt*TMath::Cos(phi0);
     110        Pval(1) = pt*TMath::Sin(phi0);
     111        Pval(2) = pt*ct;
     112        //
     113        return Pval;
     114}
     115//
    96116
    97117Double_t ObsTrk::ParToQ(TVectorD Par)
    98118{
    99   return TMath::Sign(1.0, -Par(2));
    100 }
    101 
     119        return TMath::Sign(1.0, -Par(2));
     120}
     121//
    102122TVectorD ObsTrk::GenToObsPar(TVectorD gPar, SolGridCov *GC)
    103123{
    104   TVector3 p = ParToP(gPar);
    105   Double_t pt = p.Pt();
    106   Double_t tanTh = 1.0 / TMath::Abs(gPar(4));
    107   Double_t angd = TMath::ATan(tanTh) * 180. / TMath::Pi();
    108   // Check ranges
    109   Double_t minPt = GC->GetMinPt ();
    110   if (pt < minPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << endl;
    111   Double_t maxPt = GC->GetMaxPt();
    112   if (pt > maxPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << endl;
    113   Double_t minAn = GC->GetMinAng();
    114   if (angd < minAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd
    115     << " is below grid range of " << minAn << endl;
    116   Double_t maxAn = GC->GetMaxAng();
    117   if (angd > maxAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd
    118     << " is above grid range of " << maxAn << endl;
    119   TMatrixDSym Cov = GC->GetCov(pt, angd);
    120   fCov = Cov;
    121   // Now do Choleski decomposition and random number extraction, with appropriate stabilization
    122   TMatrixDSym CvN = Cov;
    123   TMatrixDSym DCv(5); DCv.Zero();
    124   TMatrixDSym DCvInv(5); DCvInv.Zero();
    125   for (Int_t id = 0; id < 5; id++)
    126   {
    127     Double_t dVal = TMath::Sqrt(Cov(id, id));
    128     DCv   (id, id) = dVal;
    129     DCvInv(id, id) = 1.0 / dVal;
    130   }
    131   CvN.Similarity(DCvInv); // Normalize diagonal to 1
    132   TDecompChol Chl(CvN);
    133   Bool_t OK = Chl.Decompose(); // Choleski decomposition of normalized matrix
    134   TMatrixD U = Chl.GetU(); // Get Upper triangular matrix
    135   TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
    136   TVectorD r(5);
    137   for (Int_t i = 0; i < 5; i++) r(i) = gRandom->Gaus(0.0, 1.0); // Array of normal random numbers
    138   TVectorD oPar = gPar + DCv * (Ut * r); // Observed parameter vector
    139 
    140   return oPar;
    141 }
     124        TVector3 p = ParToP(gPar);
     125        Double_t pt = p.Pt();
     126        Double_t tanTh = 1.0 / TMath::Abs(gPar(4));
     127        Double_t angd = TMath::ATan(tanTh)*180. / TMath::Pi();
     128        //
     129        // Check ranges
     130        Double_t minPt = GC->GetMinPt ();
     131        if (pt < minPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << std::endl;
     132        Double_t maxPt = GC->GetMaxPt();
     133        if (pt > maxPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << std::endl;
     134        Double_t minAn = GC->GetMinAng();
     135        if (angd < minAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
     136                << " is below grid range of " << minAn << std::endl;
     137        Double_t maxAn = GC->GetMaxAng();
     138        if (angd > maxAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
     139                << " is above grid range of " << maxAn << std::endl;
     140        //
     141        TMatrixDSym Cov = GC->GetCov(pt, angd);
     142        fCov = Cov;
     143        //
     144        // Now do Choleski decomposition and random number extraction, with appropriate stabilization
     145        //
     146        TMatrixDSym CvN = Cov;
     147        TMatrixDSym DCv(5); DCv.Zero();
     148        TMatrixDSym DCvInv(5); DCvInv.Zero();
     149        for (Int_t id = 0; id < 5; id++)
     150        {
     151                Double_t dVal = TMath::Sqrt(Cov(id, id));
     152                DCv   (id, id) = dVal;
     153                DCvInv(id, id) = 1.0 / dVal;
     154        }
     155        CvN.Similarity(DCvInv);                 // Normalize diagonal to 1
     156        TDecompChol Chl(CvN);
     157        Bool_t OK = Chl.Decompose();            // Choleski decomposition of normalized matrix
     158        TMatrixD U = Chl.GetU();                        // Get Upper triangular matrix
     159        TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
     160        TVectorD r(5);
     161        for (Int_t i = 0; i < 5; i++)r(i) = gRandom->Gaus(0.0, 1.0);            // Array of normal random numbers
     162        TVectorD oPar = gPar + DCv*(Ut*r);      // Observed parameter vector
     163        //
     164        return oPar;
     165}
     166// Parameter conversion to ACTS format
     167TVectorD ObsTrk::ParToACTS(TVectorD Par)
     168{
     169        TVectorD pACTS(6);      // Return vector
     170        //
     171        Double_t b = -0.29988*fB / 2.;
     172        pACTS(0) = 1000*Par(0);         // D from m to mm
     173        pACTS(1) = 1000 * Par(3);       // z0 from m to mm
     174        pACTS(2) = Par(1);                      // Phi0 is unchanged
     175        pACTS(3) = TMath::ATan(1.0 / Par(4)) + TMath::PiOver2();                // Theta in [0, pi] range
     176        pACTS(4) = Par(2) / (b*TMath::Sqrt(1 + Par(4)*Par(4)));         // q/p in GeV
     177        pACTS(5) = 0.0;                         // Time: currently undefined
     178        //
     179        return pACTS;
     180}
     181// Covariance conversion to ACTS format
     182TMatrixDSym ObsTrk::CovToACTS(TMatrixDSym Cov)
     183{
     184        TMatrixDSym cACTS(6); cACTS.Zero();
     185        Double_t b = -0.29988*fB / 2.;
     186        //
     187        // Fill derivative matrix
     188        TMatrixD A(5, 5);       A.Zero();
     189        Double_t ct = fGenPar(4);       // cot(theta)
     190        Double_t C = fGenPar(2);                // half curvature
     191        A(0, 0) = 1000.;                // D-D  conversion to mm
     192        A(1, 2) = 1.0;          // phi0-phi0
     193        A(2, 4) = 1.0/(TMath::Sqrt(1.0 + ct*ct) * b);   // q/p-C
     194        A(3, 1) = 1000.;                // z0-z0 conversion to mm
     195        A(4, 3) = -1.0 / (1.0 + ct*ct); // theta - cot(theta)
     196        A(4, 4) = -C*ct / (b*pow(1.0 + ct*ct,3.0/2.0)); // q/p-cot(theta)
     197        //
     198        TMatrixDSym Cv = Cov;
     199        TMatrixD At(5, 5);
     200        At.Transpose(A);
     201        Cv.Similarity(At);
     202        TMatrixDSub(cACTS, 0, 4, 0, 4) = Cv;
     203        cACTS(5, 5) = 0.1;      // Currently undefined: set to arbitrary value to avoid crashes
     204        //
     205        return cACTS;
     206}
     207
     208
     209
     210       
Note: See TracChangeset for help on using the changeset viewer.