Fork me on GitHub

Changeset b750b0a in git for external/TrackCovariance


Ignore:
Timestamp:
Jan 19, 2022, 10:11:39 AM (3 years ago)
Author:
Franco BEDESCHI <bed@…>
Branches:
master
Children:
78ce8d1, 7bca620
Parents:
bd376e3
Message:

Fix CovarianceMatrix scaling error. Vertexing improvements.

Location:
external/TrackCovariance
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/TrkUtil.h

    rbd376e3 rb750b0a  
    6868        //
    6969        static TVectorD XPtoPar(TVector3 x, TVector3 p, Double_t Q, Double_t Bz);
    70         static TVector3 ParToX(TVectorD Par);                           // position of minimum distance from z axis
     70        static TVector3 ParToX(TVectorD Par);                   // position of minimum distance from z axis
    7171        static TVector3 ParToP(TVectorD Par, Double_t Bz);      // Get Momentum from track parameters
    72         static Double_t ParToQ(TVectorD Par);                           // Get track charge
     72        static Double_t ParToQ(TVectorD Par);                   // Get track charge
    7373        static void LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance);
    7474        //
    7575        // Track trajectory
    7676        //
    77         static TVector3 Xtrack(TVectorD par, Double_t s);               // Parametric track trajectory
    78         TVectorD derRphi_R(TVectorD par, Double_t R);   // Derivatives of R-phi at constant R
     77        static TVector3 Xtrack(TVectorD par, Double_t s);       // Parametric track trajectory
     78        TVectorD derRphi_R(TVectorD par, Double_t R);           // Derivatives of R-phi at constant R
    7979        TVectorD derZ_R(TVectorD par, Double_t R);              // Derivatives of z at constant R
    80         TVectorD derRphi_Z(TVectorD par, Double_t z);   // Derivatives of R-phi at constant z
     80        TVectorD derRphi_Z(TVectorD par, Double_t z);           // Derivatives of R-phi at constant z
    8181        TVectorD derR_Z(TVectorD par, Double_t z);              // Derivatives of R at constant z
    8282        //
    … …  
    8888        //
    8989        static TVectorD ParToMm(TVectorD Par);                  // Parameter conversion
    90         static TMatrixDSym CovToMm(TMatrixDSym Cov);    // Covariance conversion
     90        static TMatrixDSym CovToMm(TMatrixDSym Cov);            // Covariance conversion
    9191        //
    9292        // Inside cylindrical volume

  • external/TrackCovariance/VertexFit.cc

    rbd376e3 rb750b0a  
    1 #include <TMath.h>
     1/*
     2Vertex fitting code
     3*/
     4#include <TMath.h>
    25#include <TVectorD.h>
    36#include <TVector3.h>
    47#include <TMatrixD.h>
    58#include <TMatrixDSym.h>
     9#include <TMatrixDSymEigen.h>
    610#include "VertexFit.h"
    711//
    … …  
    3842        for (Int_t i = 0; i < fNtr; i++)
    3943        {
    40                 TVectorD pr = *trkPar[i];
    41                 fPar.push_back(new TVectorD(pr));
    42                 fParNew.push_back(new TVectorD(pr));
    43                 TMatrixDSym cv = *trkCov[i];
    44                 fCov.push_back(new TMatrixDSym(cv));
    45                 fCovNew.push_back(new TMatrixDSym(cv));
     44                fPar.push_back(new TVectorD(*trkPar[i]));
     45                fParNew.push_back(new TVectorD(*trkPar[i]));
     46                fCov.push_back(new TMatrixDSym(*trkCov[i]));
     47                fCovNew.push_back(new TMatrixDSym(*trkCov[i]));
    4648        }
    4749        fChi2List.ResizeTo(fNtr);
    … …  
    7678void VertexFit::ResetWrkArrays()
    7779{
    78         Int_t N = (Int_t)ffi.size();
    79         for (Int_t i = 0; i < N; i++)
    80         {
    81                 if (fx0i[i])  { fx0i[i]->Clear();               delete fx0i[i]; }
    82                 if (fai[i])   { fai[i]->Clear();                delete fai[i]; }
    83                 if (fdi[i])   { fdi[i]->Clear();                delete fdi[i]; }
    84                 if (fAti[i])  { fAti[i]->Clear();               delete fAti[i]; }
    85                 if (fDi[i])   { fDi[i]->Clear();                delete fDi[i]; }
    86                 if (fWi[i])   { fWi[i]->Clear();                delete fWi[i]; }
    87                 if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
    88         }
    89         fa2i.clear();
    90         fx0i.clear();
    91         fai.clear();
    92         fdi.clear();
    93         fAti.clear();
    94         fDi.clear();
    95         fWi.clear();
    96         fWinvi.clear();
     80        Int_t N = (Int_t)fdi.size();
     81        if(N > 0){
     82                for (Int_t i = 0; i < N; i++)
     83                {
     84                        if (fx0i[i])  { fx0i[i]->Clear();       delete fx0i[i]; }
     85                        if (fai[i])   { fai[i]->Clear();        delete fai[i]; }
     86                        if (fdi[i])   { fdi[i]->Clear();        delete fdi[i]; }
     87                        if (fAti[i])  { fAti[i]->Clear();       delete fAti[i]; }
     88                        if (fDi[i])   { fDi[i]->Clear();        delete fDi[i]; }
     89                        if (fWi[i])   { fWi[i]->Clear();        delete fWi[i]; }
     90                        if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
     91                }
     92                fa2i.clear();
     93                fx0i.clear();
     94                fai.clear();
     95                fdi.clear();
     96                fAti.clear();
     97                fDi.clear();
     98                fWi.clear();
     99                fWinvi.clear();
     100        }
    97101}
    98102VertexFit::~VertexFit()
    99 {
     103{       
    100104        fxCst.Clear();         
    101105        fCovCst.Clear();
    … …  
    103107        fXv.Clear();           
    104108        fcovXv.Clear();         
    105         fChi2List.Clear();     
     109        fChi2List.Clear();
    106110        //
    107111        for (Int_t i = 0; i < fNtr; i++)
    … …  
    111115                fCov[i]->Clear();               delete fCov[i];
    112116                fCovNew[i]->Clear();    delete fCovNew[i];
    113         }
     117        }       
    114118        fPar.clear();
    115119        fParNew.clear();
    … …  
    118122        //
    119123        ResetWrkArrays();
    120         ffi.clear();
     124        ffi.clear();   
    121125        fNtr = 0;
    122126}
    123127//
    124 Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
    125 {
    126         //
    127         // Find radius of minimum distance between two tracks
    128         //
    129         // p = (D,phi, C, z0, ct)
    130         //
    131         // Define arrays
    132         //
    133         Double_t C1 = p1(2);
    134         Double_t C2 = p2(2);
    135         Double_t ph1 = p1(1);
    136         Double_t ph2 = p2(1);
    137         TVectorD x0 = Fill_x0(p1);
    138         TVectorD y0 = Fill_x0(p2);
    139         TVectorD n = Fill_a(p1, 0.0);
    140         n *= (2*C1);
    141         TVectorD k = Fill_a(p2, 0.0);
    142         k *= (2*C2);
    143         //
    144         // Setup and solve linear system
    145         //
    146         Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
    147         Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
    148         Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
    149         Double_t discr = nn * kk - nk * nk;
    150         TMatrixDSym H(2);
    151         H(0, 0) = kk;
    152         H(0, 1) = nk;
    153         H(1, 0) = H(0, 1);
    154         H(1, 1) = nn;
    155         TVectorD c(2);
    156         c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
    157         c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
    158         TVectorD smin = (H * c);
    159         smin *= 1.0 / discr;
    160         //
    161         TVectorD X = x0 + smin(0) * n;
    162         TVectorD Y = y0 + smin(1) * k;
    163         //
    164         // Higher order corrections
    165         X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1);
    166         X(1) +=  C1 * smin(0) * smin(0) * TMath::Cos(ph1);
    167         Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2);
    168         Y(1) +=  C2 * smin(1) * smin(1) * TMath::Cos(ph2);
    169         //
    170         TVectorD Xavg = 0.5 * (X + Y);
    171         //
    172         //
    173         return TMath::Sqrt(Xavg(0)*Xavg(0)+Xavg(1)*Xavg(1));
    174 }
    175 //
    176 // Starting radius determination
    177 Double_t VertexFit::StartRadius()
    178 {
    179         //
    180         // Maximum impact parameter
    181         Double_t Rd = 0;
    182         for (Int_t i = 0; i < fNtr; i++)
    183         {
    184                 TVectorD par = *fPar[i];
    185                 Double_t Dabs = TMath::Abs(par(0));
    186                 if (Dabs > Rd)Rd = Dabs;
    187         }
    188         //-----------------------------
    189         //
    190         // Find track pair with phi difference closest to pi/2
    191         Int_t isel = 0; Int_t jsel = 0;         // selected track indices
    192         Double_t dSinMax = 0.0;                         // Max phi difference
    193         for (Int_t i = 0; i < fNtr - 1; i++)
    194         {
    195                 TVectorD pari = *fPar[i];
    196                 Double_t phi1 = pari(1);
    197 
    198                 for (Int_t j = i + 1; j < fNtr; j++)
    199                 {
    200                         TVectorD parj = *fPar[j];
    201                         Double_t phi2 = parj(1);
    202                         Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1));
    203                         if (Sindphi > dSinMax)
    204                         {
    205                                 isel = i; jsel = j;
    206                                 dSinMax = Sindphi;
    207                         }
    208                 }
    209         }
    210         //
    211         //------------------------------------------
    212         //
    213         // Find radius of minimum distrance between tracks
    214         TVectorD p1 = *fPar[isel];
    215         TVectorD p2 = *fPar[jsel];
    216         Double_t R = FastRv(p1, p2);
    217         //
    218         R = 0.9 * R + 0.1 * Rd;         // Protect for overshoot
    219         //
    220         return R;
    221 }
    222 //
    223 // Regularized symmetric matrix inversion
    224 //
    225 TMatrixDSym VertexFit::RegInv(TMatrixDSym& Min)
    226 {
    227         TMatrixDSym M = Min;                            // Decouple from input
    228         Int_t N = M.GetNrows();                 // Matrix size
    229         TMatrixDSym D(N); D.Zero();             // Normaliztion matrix
    230         TMatrixDSym R(N);                               // Normarized matrix
    231         TMatrixDSym Rinv(N);                            // Inverse of R
    232         TMatrixDSym Minv(N);                            // Inverse of M
    233         //
    234         // Check for 0's and normalize
    235         for (Int_t i = 0; i < N; i++)
    236         {
    237                 if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
    238                 else D(i, i) = 1.0;
    239         }
    240         R = M.Similarity(D);
    241         //
    242         // Recursive algorithms stops when N = 2
    243         //
    244         //****************
    245         // case N = 2  ***
    246         //****************
    247         if (N == 2)
    248         {
    249                 Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
    250                 if (det == 0)
    251                 {
    252                         std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
    253                         Rinv.Zero();    // Return null matrix
    254                 }
    255                 else
    256                 {
    257                         // invert matrix
    258                         Rinv(0, 0) = R(1, 1);
    259                         Rinv(0, 1) = -R(0, 1);
    260                         Rinv(1, 0) = Rinv(0, 1);
    261                         Rinv(1, 1) = R(0, 0);
    262                         Rinv *= 1. / det;
    263                 }
    264         }
    265         //****************
    266         // case N > 2  ***
    267         //****************
    268         else
    269         {
    270                 // Break up matrix
    271                 TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2);   // Upper left
    272                 TVectorD p(N - 1);
    273                 for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
    274                 Double_t q = R(N - 1, N - 1);
    275                 //Invert pieces and re-assemble
    276                 TMatrixDSym Ainv(N - 1);
    277                 TMatrixDSym A(N - 1);
    278                 if (TMath::Abs(q) > 1.0e-15)
    279                 {
    280                         // Case |q| > 0
    281                         Ainv.Rank1Update(p, -1.0 / q);
    282                         Ainv += Q;
    283                         A = RegInv(Ainv);               // Recursive call
    284                         TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
    285                         //
    286                         TVectorD b = (-1.0 / q) * (A * p);
    287                         for (Int_t i = 0; i < N - 1; i++)
    288                         {
    289                                 Rinv(N - 1, i) = b(i);
    290                                 Rinv(i, N - 1) = b(i);
    291                         }
    292                         //
    293                         Double_t pdotb = 0.;
    294                         for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
    295                         Double_t c = (1.0 - pdotb) / q;
    296                         Rinv(N - 1, N - 1) = c;
    297                 }
    298                 else
    299                 {
    300                         // case q = 0
    301                         TMatrixDSym Qinv = RegInv(Q);           // Recursive call
    302                         Double_t a = Qinv.Similarity(p);
    303                         Double_t c = -1.0 / a;
    304                         Rinv(N - 1, N - 1) = c;
    305                         //
    306                         TVectorD b = (1.0 / a) * (Qinv * p);
    307                         for (Int_t i = 0; i < N - 1; i++)
    308                         {
    309                                 Rinv(N - 1, i) = b(i);
    310                                 Rinv(i, N - 1) = b(i);
    311                         }
    312                         //
    313                         A.Rank1Update(p, -1 / a);
    314                         A += Q;
    315                         A.Similarity(Qinv);
    316                         TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
    317                 }
    318         }
    319         Minv = Rinv.Similarity(D);
    320         return Minv;
    321 }
    322 //
    323 //
    324 //
    325 TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
    326 {
    327         //
    328         // Derivative of track 3D position vector with respect to track parameters at constant phase
    329         //
    330         // par = vector of track parameters
    331         // phi = phase
    332         //
    333         TMatrixD A(3, 5);
     128//
     129//
     130TVectorD VertexFit::Fill_x0(TVectorD par)
     131{
     132        //
     133        // Calculate track 3D position at R = |D| (minimum approach to z-axis)
     134        //
     135        TVectorD x0(3);
    334136        //
    335137        // Decode input arrays
    … …  
    341143        Double_t ct = par(4);
    342144        //
    343         // Fill derivative matrix dx/d alpha
    344         // D
    345         A(0, 0) = -TMath::Sin(p0);
    346         A(1, 0) = TMath::Cos(p0);
    347         A(2, 0) = 0.0;
    348         // phi0
    349         A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
    350         A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    351         A(2, 1) = 0.0;
    352         // C
    353         A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
    354         A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
    355         A(2, 2) = -ct * phi / (2 * C * C);
    356         // z0
    357         A(0, 3) = 0.0;
    358         A(1, 3) = 0.0;
    359         A(2, 3) = 1.0;
    360         // ct = lambda
    361         A(0, 4) = 0.0;
    362         A(1, 4) = 0.0;
    363         A(2, 4) = phi / (2 * C);
    364         //
    365         return A;
    366 }
    367 //
    368 TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
    369 {
    370         //
    371         // Derivative of track 3D position vector with respect to phase at constant track parameters
    372         //
    373         // par = vector of track parameters
    374         // phi = phase
    375         //
    376         TVectorD a(3);
     145        x0(0) = -D * TMath::Sin(p0);
     146        x0(1) = D * TMath::Cos(p0);
     147        x0(2) = z0;
     148        //
     149        return x0;
     150}
     151//
     152TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
     153{
     154        //
     155        // Calculate track 3D position for a given phase, phi
     156        //
     157        TVectorD x(3);
    377158        //
    378159        // Decode input arrays
    … …  
    384165        Double_t ct = par(4);
    385166        //
    386         a(0) = TMath::Cos(phi + p0) / (2 * C);
    387         a(1) = TMath::Sin(phi + p0) / (2 * C);
    388         a(2) = ct / (2 * C);
    389         //
    390         return a;
    391 }
    392 //
    393 TVectorD VertexFit::Fill_x0(TVectorD par)
    394 {
    395         //
    396         // Calculate track 3D position at R = |D| (minimum approach to z-axis)
    397         //
    398         TVectorD x0(3);
    399         //
    400         // Decode input arrays
    401         //
    402         Double_t D = par(0);
    403         Double_t p0 = par(1);
    404         Double_t C = par(2);
    405         Double_t z0 = par(3);
    406         Double_t ct = par(4);
    407         //
    408         x0(0) = -D * TMath::Sin(p0);
    409         x0(1) = D * TMath::Cos(p0);
    410         x0(2) = z0;
    411         //
    412         return x0;
    413 }
    414 //
    415 TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
    416 {
    417         //
    418         // Calculate track 3D position for a given phase, phi
    419         //
    420         TVectorD x(3);
    421         //
    422         // Decode input arrays
    423         //
    424         Double_t D = par(0);
    425         Double_t p0 = par(1);
    426         Double_t C = par(2);
    427         Double_t z0 = par(3);
    428         Double_t ct = par(4);
    429         //
    430167        TVectorD x0 = Fill_x0(par);
    431168        x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    … …  
    439176{
    440177        //
    441         // Get track parameters and their covariance
     178        // Get track parameters, covariance and phase
     179        Double_t fs = ffi[i];                   // Get phase
    442180        TVectorD par = *fParNew[i];
    443181        TMatrixDSym Cov = *fCov[i];
    444182        //
    445183        // Fill all track related work arrays arrays
    446         Double_t fs = ffi[i];                                           // Get phase
     184        TMatrixD A = derXdPar(par, fs);         // A = dx/da = derivatives wrt track parameters
     185        TMatrixDSym Winv = Cov;                         
     186        Winv.Similarity(A);                     // W^-1 = A*C*A'
     187
     188        TMatrixD At(TMatrixD::kTransposed, A);          // A transposed
     189        fAti.push_back(new TMatrixD(At));               // Store A'
     190        fWinvi.push_back(new TMatrixDSym(Winv));        // Store W^-1 matrix
     191        //
    447192        TVectorD xs = Fill_x(par, fs);
    448193        fx0i.push_back(new TVectorD(xs));                       // Start helix position
    449         //
    450         TMatrixD A = Fill_A(par, fs);                           // A = dx/da = derivatives wrt track parameters
    451         TMatrixD At(TMatrixD::kTransposed, A);          // A transposed
    452         fAti.push_back(new TMatrixD(At));                       // Store A'
    453         TVectorD di = A * (par - *fPar[i]);             // x-shift
    454         fdi.push_back(new TVectorD(di));                        // Store x-shift
    455         TMatrixDSym Winv = Cov.Similarity(A);           // W^-1 = A*C*A'
    456         fWinvi.push_back(new TMatrixDSym(Winv));        // Store W^-1 matrix
     194        //
     195        TVectorD di = A * (par - *fPar[i]);                     // x-shift
     196        fdi.push_back(new TVectorD(di));                        // Store x-shift       
    457197        //
    458198        TMatrixDSym W = RegInv(Winv);                           // W = (A*C*A')^-1
    459199        fWi.push_back(new TMatrixDSym(W));                      // Store W matrix
    460200        //
    461         TVectorD a = Fill_a(par, fs);                           // a = dx/ds = derivatives wrt phase
     201        TVectorD a = derXds(par, fs);                           // a = dx/ds = derivatives wrt phase
    462202        fai.push_back(new TVectorD(a));                         // Store a
    463203        //
    … …  
    473213}
    474214//
     215void VertexFit::VtxFitNoSteer()
     216{
     217        //
     218        // Initialize
     219        //
     220        std::vector<TVectorD*> x0i;                             // Tracks at ma
     221        std::vector<TVectorD*> ni;                              // Track derivative wrt phase
     222        std::vector<TMatrixDSym*> Ci;                   // Position error matrix at fixed phase
     223        std::vector<TVectorD*> wi;                              // Ci*ni
     224        //
     225        // Track loop
     226        for (Int_t i = 0; i < fNtr; i++)
     227        {
     228                Double_t s = 0.;
     229                TVectorD par = *fPar[i];
     230                TMatrixDSym Cov = *fCov[i];
     231                x0i.push_back(new TVectorD(Fill_x0(par)));
     232                ni.push_back(new TVectorD(derXds(par, s)));
     233                TMatrixD A = derXdPar(par, s);
     234                Ci.push_back(new TMatrixDSym(Cov.Similarity(A)));
     235                TMatrixDSym Cinv = RegInv(*Ci[i]);
     236                wi.push_back(new TVectorD(Cinv * (*ni[i])));
     237        }
     238        //std::cout << "Vtx init completed. fNtr = "<<fNtr << std::endl;
     239        //
     240        // Get fit vertex
     241        //
     242        TMatrixDSym D(3); D.Zero();
     243        TVectorD Dx(3); Dx.Zero();
     244        for (Int_t i = 0; i < fNtr; i++)
     245        {
     246                TMatrixDSym Cinv = RegInv(*Ci[i]);
     247                TMatrixDSym W(3);
     248                W.Rank1Update(*wi[i], 1. / Ci[i]->Similarity(*wi[i]));
     249                TMatrixDSym Dd = Cinv - W;
     250                D += Dd;
     251                Dx += Dd * (*x0i[i]);
     252        }
     253        if(fVtxCst){
     254                D  += fCovCstInv;
     255                Dx += fCovCstInv*fxCst;
     256        }
     257        fXv = RegInv(D) * Dx;
     258        //std::cout << "Fast vertex (x, y, z) = "<<fXv(0)<<", "<<fXv(1)<<", "<<fXv(2) << std::endl;
     259        //
     260        // Get fit phases
     261        //
     262        for (Int_t i = 0; i < fNtr; i++){
     263                Double_t si = Dot(*wi[i], fXv - (*x0i[i])) / Ci[i]->Similarity(*wi[i]);
     264                ffi.push_back(si);
     265                //TVectorD xvi = Fill_x(*fPar[i],si);
     266                //std::cout << "Fast vertex "<<i<<": xvi = "<<xvi(0)<<", "<<xvi(1)<<", "<<xvi(2)
     267                //      <<", si = "<<si<< std::endl;
     268        }
     269        //
     270        // Cleanup
     271        for (Int_t i = 0; i < fNtr; i++)
     272        {
     273                x0i[i]->Clear();        delete x0i[i];
     274                ni[i]->Clear();         delete ni[i];
     275                Ci[i]->Clear();         delete Ci[i];
     276                wi[i]->Clear();         delete wi[i];
     277        }
     278        x0i.clear();;
     279        ni.clear();
     280        Ci.clear();
     281        wi.clear();
     282}
     283//
    475284void  VertexFit::VertexFitter()
    476285{
    … …  
    487296        TMatrixDSym covX(3);
    488297        Double_t Chi2 = 0;
    489         TVectorD x0 = fXv;      // If previous fit done
    490         if (fRold < 0.0) {
    491                 // External constraint
    492                 if (fVtxCst) fRold = TMath::Sqrt(fxCst(0) * fxCst(0) + fxCst(1) * fxCst(1));
    493                 // No constraint
    494                 else for (Int_t i = 0; i < 3; i++)x0(i) = 1000.;        // Set to arbitrary large value
    495         }
    496         //
    497         // Starting vertex radius approximation
    498         //
    499         Double_t R = fRold;                                             // Use previous fit if available
    500         if (R < 0.0) R = StartRadius();                 // Rough vertex estimate
    501         //std::cout << "Start radius: " << R << std::endl;
     298        //
     299        VtxFitNoSteer();        // Fast vertex finder on first pass (set ffi and fXv)
     300        TVectorD x0 = fXv;
    502301        //
    503302        // Iteration properties
    … …  
    515314                TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
    516315                covX.Zero();            // Reset vertex covariance
    517                 cterm.Zero();   // Reset constant term
     316                cterm.Zero();           // Reset constant term
    518317                H.Zero();               // Reset H matrix
    519318                DW1D.Zero();
    … …  
    530329                        TVectorD par = *fPar[i];
    531330                        TMatrixDSym Cov = *fCov[i];
    532                         //
    533                         // For first iteration only
    534                         Double_t fs;
    535                         if (Ntry <= 0)  // Initialize all phases on first pass
    536                         {
    537                                 Double_t D = par(0);
    538                                 Double_t C = par(2);
    539                                 Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
    540                                 fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
    541                                 ffi.push_back(fs);
    542                         }
    543331                        //
    544332                        // Update track related arrays
    … …  
    555343                        // update constant term
    556344                        TVectorD xs = *fx0i[i] - *fdi[i];
    557                         TVectorD xx0 = *fx0i[i];
    558                         /*
    559                         std::cout << "Iter. " << Ntry << ", trk " << i << ", x= "
    560                                 << xx0(0) << ", " << xx0(1) << ", " << xx0(2)<<
    561                                 ", ph0= "<<par(1)<< std::endl;
    562                         */
     345                        //TVectorD xx0 = *fx0i[i];
     346                       
     347                        //std::cout << "Iter. " << Ntry << ", trk " << i << ", xs= "
     348                        //      << xs(0) << ", " << xs(1) << ", " << xs(2)<<
     349                        //      ", ph0= "<<par(1)<< std::endl;
     350                       
    563351                        cterm += Ds * xs;
    564352                }                               // End loop on tracks
    … …  
    584372                        TMatrixDSym Wm1 = *fWinvi[i];
    585373                        fChi2List(i) = Wm1.Similarity(lambda);
     374                        if(fChi2List(i) < 0.0){
     375                                //std::cout<<"# "<<i<<", Chi2= "<<fChi2List(i)<<", Wm1:"<<std::endl; Wm1.Print();
     376                                //std::cout<<"Lambda= "<<std::endl; lambda.Print();
     377                        }
    586378                        Chi2 += fChi2List(i);
    587379                        TVectorD a = *fai[i];
    588                         TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));
    589                         for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
     380                        TVectorD b = (*fWi[i]) * (x - *fx0i[i] + *fdi[i]);
     381                        ffi[i] += Dot(a, b) / fa2i[i];
    590382                        TVectorD newPar = *fPar[i] - ((*fCov[i]) * (*fAti[i])) * lambda;
    591383                        fParNew[i] = new TVectorD(newPar);
    … …  
    610402        //
    611403        fVtxDone = kTRUE;               // Set fit completion flag
    612         fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1));     // Store fit radius
     404        fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1));     // Store fit
     405        //std::cout << "Found vertex " << fXv(0) << ", " << fXv(1) << ", " << fXv(2)
     406        //      << ", after "<<Ntry<<" iterations"<<std::endl;
    613407        //
    614408}

  • external/TrackCovariance/VertexFit.h

    rbd376e3 rb750b0a  
    66#include <TVectorD.h>
    77#include <TMatrixDSym.h>
     8#include "TrkUtil.h"
    89#include "ObsTrk.h"
    910#include <vector>
    … …  
    1213// Class for vertex fitting
    1314
    14 class VertexFit {
     15class VertexFit: public TrkUtil
     16{
    1517        //
    1618        // Vertex fitting with track parameters steering
    … …  
    2123        //
    2224        // Inputs
    23         Int_t fNtr;                                     // Number of tracks
     25        Int_t fNtr;                                                     // Number of tracks
    2426        std::vector<TVectorD*> fPar;            // Input parameter array
    2527        std::vector<TVectorD*> fParNew;         // Updated parameter array
    … …  
    2729        std::vector<TMatrixDSym*> fCovNew;      // Updated parameter covariances
    2830        // Constraints
    29         Bool_t fVtxCst;                         // Vertex constraint flag
    30         TVectorD fxCst;                         // Constraint value
     31        Bool_t fVtxCst;                                 // Vertex constraint flag
     32        TVectorD fxCst;                                 // Constraint value
    3133        TMatrixDSym fCovCst;                    // Constraint
    3234        TMatrixDSym fCovCstInv;                 // Inverse of constraint covariance
    3335        //
    3436        // Results
    35         Bool_t fVtxDone;                                // Flag vertex fit completed
     37        Bool_t fVtxDone;                        // Flag vertex fit completed
    3638        Double_t fRold;                         // Current value of vertex radius
    3739        TVectorD fXv;                           // Found vertex
    … …  
    5254        //
    5355        // Service routines
    54         //void InitWrkArrays();                                                 // Initializations
    5556        void ResetWrkArrays();                                                  // Clear work arrays
    56         Double_t StartRadius();                                                 // Starting vertex radius determination
    57         Double_t FastRv(TVectorD p1, TVectorD p2);              // Fast vertex radius determination
    58         TMatrixDSym RegInv(TMatrixDSym& Smat0);                 // Regularized 3D matrix inversion
    59         TMatrixD Fill_A(TVectorD par, Double_t phi);            // Derivative of track position wrt track parameters
    60         TVectorD Fill_a(TVectorD par, Double_t phi);            // Derivative of track position wrt track phase
     57        //Double_t StartRadius();                                                       // Starting vertex radius determination
     58        //Double_t FastRv(TVectorD p1, TVectorD p2);            // Fast vertex radius determination
     59        //TMatrixDSym RegInv(TMatrixDSym& Smat0);               // Regularized 3D matrix inversion
     60        //TMatrixD Fill_A(TVectorD par, Double_t phi);  // Derivative of track position wrt track parameters
     61        //TVectorD Fill_a(TVectorD par, Double_t phi);  // Derivative of track position wrt track phase
    6162        TVectorD Fill_x0(TVectorD par);                                 // Track position at dma to z-axis
    62         TVectorD Fill_x(TVectorD par, Double_t phi);            // Track position at given phase
     63        TVectorD Fill_x(TVectorD par, Double_t phi);    // Track position at given phase
    6364        void UpdateTrkArrays(Int_t i);                                  // Fill track realted arrays
    64         void VertexFitter();                                                            // Vertex finder routine
     65        void VtxFitNoSteer();                                                   // Vertex fitter routine w/o parameter steering
     66        void VertexFitter();                                                    // Vertex fitter routine w/  parameter steering
    6567public:
    6668        //
Note: See TracChangeset for help on using the changeset viewer.