Fork me on GitHub

Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/VertexFit.cc

    r127644a r82db145  
    88// Constructors
    99//
    10 // Empty
     10//
     11// Empty construction (to be used when adding tracks later with AddTrk() )
    1112VertexFit::VertexFit()
    1213{
    1314        fNtr = 0;
     15        fRold = -1.0;
    1416        fVtxDone = kFALSE;
    1517        fVtxCst = kFALSE;
    1618        fxCst.ResizeTo(3);
    17         fCovCst.ResizeTo(3,3);
     19        fCovCst.ResizeTo(3, 3);
    1820        fXv.ResizeTo(3);
    19         fcovXv.ResizeTo(3,3);
    20 }
    21 // Parameters and covariances
     21        fcovXv.ResizeTo(3, 3);
     22}
     23//
     24// Build from list of parameters and covariances
    2225VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
    2326{
    2427        fNtr = Ntr;
     28        fRold = -1.0;
    2529        fVtxDone = kFALSE;
    2630        fVtxCst = kFALSE;
    2731        fxCst.ResizeTo(3);
    28         fCovCst.ResizeTo(3,3);
     32        fCovCst.ResizeTo(3, 3);
    2933        fXv.ResizeTo(3);
    30         fcovXv.ResizeTo(3,3);
    31         //
    32         fPar = trkPar;
    33         fCov = trkCov;
    34         fChi2List.ResizeTo(Ntr);
    35         //
    36         ffi = new Double_t[Ntr];                                // Fit phases
    37         fx0i = new TVectorD* [Ntr];                     // Track expansion points
    38         for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
    39         fai = new TVectorD * [Ntr];                     // dx/dphi
    40         for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
    41         fa2i = new Double_t[Ntr];                               // a'Wa
    42         fDi = new TMatrixDSym*[Ntr];            // W-WBW
    43         for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
    44         fWi = new TMatrixDSym * [Ntr];  // (ACA')^-1
    45         for (Int_t i = 0; i < Ntr; i++) fWi[i] = new TMatrixDSym(3);
    46         fWinvi = new TMatrixDSym * [Ntr];       // ACA'
    47         for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
    48 
    49 }
    50 // ObsTrk list
     34        fcovXv.ResizeTo(3, 3);
     35        //
     36        for (Int_t i = 0; i < fNtr; i++)
     37        {
     38                TVectorD pr = *trkPar[i];
     39                fPar.push_back(new TVectorD(pr));
     40                TMatrixDSym cv = *trkCov[i];
     41                fCov.push_back(new TMatrixDSym(cv));
     42        }
     43        fChi2List.ResizeTo(fNtr);
     44        //
     45}
     46//
     47// Build from ObsTrk list of tracks
    5148VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
    5249{
    5350        fNtr = Ntr;
     51        fRold = -1.0;
    5452        fVtxDone = kFALSE;
    5553        fVtxCst = kFALSE;
    5654        fxCst.ResizeTo(3);
    57         fCovCst.ResizeTo(3,3);
     55        fCovCst.ResizeTo(3, 3);
    5856        fXv.ResizeTo(3);
    59         fcovXv.ResizeTo(3,3);
    60         //
    61         fPar = new TVectorD*[Ntr];
    62         fCov = new TMatrixDSym*[Ntr];
    63         fChi2List.ResizeTo(Ntr);
    64         for (Int_t i = 0; i < Ntr; i++)
    65         {
    66                 fPar[i] = new TVectorD(track[i]->GetObsPar());
    67                 //std::cout << "fPar = " << std::endl; fPar[i]->Print();
    68                 fCov[i] = new TMatrixDSym(track[i]->GetCov());
    69                 //std::cout << "fCov = " << std::endl; fCov[i]->Print();
    70         }
    71         //
    72         ffi = new Double_t[Ntr];                                // Fit phases
    73         fx0i = new TVectorD * [Ntr];                    // Track expansion points
    74         for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
    75         fai = new TVectorD * [Ntr];                     // dx/dphi
    76         for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
    77         fa2i = new Double_t[Ntr];                               // a'Wa
    78         fDi = new TMatrixDSym * [Ntr];  // W-WBW
    79         for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
    80         fWi = new TMatrixDSym * [Ntr];  // (ACA')^-1
    81         for (Int_t i = 0; i < Ntr; i++) fWi[i] = new TMatrixDSym(3);
    82         fWinvi = new TMatrixDSym * [Ntr];       // ACA'
    83         for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
    84 
    85         //std::cout << "VertexFit constructor executed" << std::endl;
     57        fcovXv.ResizeTo(3, 3);
     58        //
     59        fChi2List.ResizeTo(fNtr);
     60        for (Int_t i = 0; i < fNtr; i++)
     61        {
     62                fPar.push_back(new TVectorD(track[i]->GetObsPar()));
     63                fCov.push_back(new TMatrixDSym(track[i]->GetCov()));
     64        }
    8665}
    8766//
    8867// Destructor
     68//
     69void VertexFit::ResetWrkArrays()
     70{
     71        Int_t N = (Int_t)ffi.size();
     72        for (Int_t i = 0; i < N; i++)
     73        {
     74                if (fx0i[i])  { fx0i[i]->Clear();               delete fx0i[i]; }
     75                if (fai[i])   { fai[i]->Clear();                        delete fai[i]; }
     76                if (fDi[i])   { fDi[i]->Clear();                        delete fDi[i]; }
     77                if (fWi[i])   { fWi[i]->Clear();                        delete fWi[i]; }
     78                if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
     79        }
     80        fa2i.clear();
     81        fx0i.clear();
     82        fai.clear();
     83        fDi.clear();
     84        fWi.clear();
     85        fWinvi.clear();
     86}
    8987VertexFit::~VertexFit()
    9088{
    91         fxCst.Clear();
    92         fCovCst.Clear();
    93         fXv.Clear();
    94         fcovXv.Clear();
    95         fChi2List.Clear();
    96         //
    97         for (Int_t i= 0; i < fNtr; i++)
    98         {
    99                 fPar[i]->Clear();
    100                 fCov[i]->Clear();
    101                 //
    102                 fx0i[i]->Clear(); delete fx0i[i];
    103                 fai[i]->Clear(); delete fai[i];
    104                 fDi[i]->Clear(); delete fDi[i];
    105                 fWi[i]->Clear(); delete fWi[i];
    106                 fWinvi[i]->Clear();     delete fWinvi[i];
    107         }
     89        fxCst.Clear();         
     90        fCovCst.Clear();               
     91        fXv.Clear();           
     92        fcovXv.Clear();         
     93        fChi2List.Clear();     
     94        //
     95        for (Int_t i = 0; i < fNtr; i++)
     96        {
     97                fPar[i]->Clear();       delete fPar[i];
     98                fCov[i]->Clear();       delete fCov[i];
     99        }
     100        fPar.clear();
     101        fCov.clear();   
     102        //
     103        ResetWrkArrays();
     104        ffi.clear();
    108105        fNtr = 0;
    109         delete[] fPar;
    110         delete[] fCov;
    111         delete[] ffi;
    112         delete[] fa2i;
    113         delete[] fx0i;
    114         delete[] fai;
    115         delete[] fDi;
    116         delete[] fWi;
    117         delete[] fWinvi;
    118 }
    119 //
    120 Double_t VertexFit::FastRv1(TVectorD p1, TVectorD p2)
    121 {
    122         //
    123         // Find radius of intersection between two tracks in the transverse plane
     106}
     107//
     108Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
     109{
     110        //
     111        // Find radius of minimum distance between two tracks
    124112        //
    125113        // p = (D,phi, C, z0, ct)
    … …  
    127115        // Define arrays
    128116        //
    129         Double_t r1 = 1.0 / p1(2);
    130         Double_t r2 = 1.0 / p2(2);
     117        Double_t C1 = p1(2);
     118        Double_t C2 = p2(2);
     119        Double_t ph1 = p1(1);
     120        Double_t ph2 = p2(1);
    131121        TVectorD x0 = Fill_x0(p1);
    132122        TVectorD y0 = Fill_x0(p2);
    133123        TVectorD n = Fill_a(p1, 0.0);
    134         n *= r1;
     124        n *= (2*C1);
    135125        TVectorD k = Fill_a(p2, 0.0);
    136         k *= r2;
     126        k *= (2*C2);
    137127        //
    138128        // Setup and solve linear system
    … …  
    148138        H(1, 1) = nn;
    149139        TVectorD c(2);
    150         c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) +=  n(i) * (y0(i) - x0(i));
     140        c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
    151141        c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
    152142        TVectorD smin = (H * c);
    … …  
    155145        TVectorD X = x0 + smin(0) * n;
    156146        TVectorD Y = y0 + smin(1) * k;
    157         Double_t R1 = TMath::Sqrt(X(0) * X(0) + X(1) * X(1));
    158         Double_t R2 = TMath::Sqrt(Y(0) * Y(0) + Y(1) * Y(1));
    159         //
    160         return 0.5*(R1+R2);
    161 }
    162 Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
    163 {
    164         //
    165         // Find radius of minimum distance
    166         //
    167         // p = (D,phi, C)
    168         //
    169         // Solving matrix
    170         TMatrixDSym H(2);
    171         H(0, 0) = -TMath::Cos(p2(1));
    172         H(0, 1) =  TMath::Cos(p1(1));
    173         H(1, 0) = -TMath::Sin(p2(1));
    174         H(1, 1) =  TMath::Sin(p1(1));
    175         Double_t Det = TMath::Sin(p2(1) - p1(1));
    176         H *= 1.0 / Det;
    177         //
    178         // Convergence parameters
    179         Int_t Ntry = 0;
    180         Int_t NtryMax = 100;
    181         Double_t eps = 1000.;
    182         Double_t epsMin = 1.0e-6;
    183         //
    184         // Vertex finding loop
    185         //
    186         TVectorD cterm(2);
    187         cterm(0) = p1(0);
    188         cterm(1) = p2(0);
    189         TVectorD xv(2);
    190         Double_t R = 1000.;
    191         while (eps > epsMin)
    192         {
    193                 xv = H * cterm;
    194                 Ntry++;
    195                 if (Ntry > NtryMax)
     147        //
     148        // Higher order corrections
     149        X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1);
     150        X(1) +=  C1 * smin(0) * smin(0) * TMath::Cos(ph1);
     151        Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2);
     152        Y(1) +=  C2 * smin(1) * smin(1) * TMath::Cos(ph2);
     153        //
     154        TVectorD Xavg = 0.5 * (X + Y);
     155        //
     156        //
     157        return TMath::Sqrt(Xavg(0)*Xavg(0)+Xavg(1)*Xavg(1));
     158}
     159//
     160// Starting radius determination
     161Double_t VertexFit::StartRadius()
     162{
     163        //
     164        // Maximum impact parameter
     165        Double_t Rd = 0;
     166        for (Int_t i = 0; i < fNtr; i++)
     167        {
     168                TVectorD par = *fPar[i];
     169                Double_t Dabs = TMath::Abs(par(0));
     170                if (Dabs > Rd)Rd = Dabs;
     171        }
     172        //-----------------------------
     173        //
     174        // Find track pair with phi difference closest to pi/2
     175        Int_t isel = 0; Int_t jsel = 0;         // selected track indices
     176        Double_t dSinMax = 0.0;                         // Max phi difference
     177        for (Int_t i = 0; i < fNtr - 1; i++)
     178        {
     179                TVectorD pari = *fPar[i];
     180                Double_t phi1 = pari(1);
     181
     182                for (Int_t j = i + 1; j < fNtr; j++)
    196183                {
    197                         std::cout << "FastRv: maximum number of iteration reached" << std::endl;
    198                         break;
     184                        TVectorD parj = *fPar[j];
     185                        Double_t phi2 = parj(1);
     186                        Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1));
     187                        if (Sindphi > dSinMax)
     188                        {
     189                                isel = i; jsel = j;
     190                                dSinMax = Sindphi;
     191                        }
    199192                }
    200                 Double_t Rnew = TMath::Sqrt(xv(0) * xv(0) + xv(1) * xv(1));
    201                 eps = Rnew - R;
    202                 R = Rnew;
    203                 cterm(0) = p1(2) * R * R;
    204                 cterm(1) = p2(2) * R * R;
    205         }
     193        }
     194        //
     195        //------------------------------------------
     196        //
     197        // Find radius of minimum distrance between tracks
     198        TVectorD p1 = *fPar[isel];
     199        TVectorD p2 = *fPar[jsel];
     200        Double_t R = FastRv(p1, p2);
     201        //
     202        R = 0.9 * R + 0.1 * Rd;         // Protect for overshoot
    206203        //
    207204        return R;
    208205}
    209 
    210 TMatrixDSym VertexFit::RegInv3(TMatrixDSym &Smat0)
    211 {
    212         //
    213         // Regularized inversion of symmetric 3x3 matrix with positive diagonal elements
    214         //
    215         TMatrixDSym Smat = Smat0;
    216         Int_t N = Smat.GetNrows();
    217         if (N != 3)
    218         {
    219                 std::cout << "RegInv3 called with  matrix size != 3. Abort & return standard inversion." << std::endl;
    220                 return Smat.Invert();
    221         }
    222         TMatrixDSym D(N); D.Zero();
    223         Bool_t dZero = kTRUE;   // No elements less or equal 0 on the diagonal
    224         for (Int_t i = 0; i < N; i++) if (Smat(i, i) <= 0.0)dZero = kFALSE;
    225         if (dZero)
    226         {
    227                 for (Int_t i = 0; i < N; i++) D(i, i) = 1.0 / TMath::Sqrt(Smat(i, i));
    228                 TMatrixDSym RegMat = Smat.Similarity(D);
    229                 TMatrixDSym Q(2);
    230                 for (Int_t i = 0; i < 2; i++)
     206//
     207// Regularized symmetric matrix inversion
     208//
     209TMatrixDSym VertexFit::RegInv(TMatrixDSym& Min)
     210{
     211        TMatrixDSym M = Min;                            // Decouple from input
     212        Int_t N = M.GetNrows();                 // Matrix size
     213        TMatrixDSym D(N); D.Zero();             // Normaliztion matrix
     214        TMatrixDSym R(N);                               // Normarized matrix
     215        TMatrixDSym Rinv(N);                            // Inverse of R
     216        TMatrixDSym Minv(N);                            // Inverse of M
     217        //
     218        // Check for 0's and normalize
     219        for (Int_t i = 0; i < N; i++)
     220        {
     221                if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
     222                else D(i, i) = 1.0;
     223        }
     224        R = M.Similarity(D);
     225        //
     226        // Recursive algorithms stops when N = 2
     227        //
     228        //****************
     229        // case N = 2  ***
     230        //****************
     231        if (N == 2)
     232        {
     233                Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
     234                if (det == 0)
    231235                {
    232                         for (Int_t j = 0; j < 2; j++)Q(i, j) = RegMat(i, j);
     236                        std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
     237                        Rinv.Zero();    // Return null matrix
    233238                }
    234                 Double_t Det = 1 - Q(0, 1)*Q(1, 0);
    235                 TMatrixDSym H(2);
    236                 H = Q;
    237                 H(0, 1) = -Q(0, 1);
    238                 H(1, 0) = -Q(1, 0);
    239                 TVectorD p(2);
    240                 p(0) = RegMat(0, 2);
    241                 p(1) = RegMat(1, 2);
    242                 Double_t pHp = H.Similarity(p);
    243                 Double_t h = pHp-Det;
    244                 //
    245                 TMatrixDSym pp(2); pp.Rank1Update(p);
    246                 TMatrixDSym F = (h*H) - pp.Similarity(H);
    247                 F *= 1.0 / Det;
    248                 TVectorD b = H*p;
    249                 TMatrixDSym InvReg(3);
    250                 for (Int_t i = 0; i < 2; i++)
     239                else
    251240                {
    252                         InvReg(i, 2) = b(i);
    253                         InvReg(2, i) = b(i);
    254                         for (Int_t j = 0; j < 2; j++) InvReg(i, j) = F(i, j);
     241                        // invert matrix
     242                        Rinv(0, 0) = R(1, 1);
     243                        Rinv(0, 1) = -R(0, 1);
     244                        Rinv(1, 0) = Rinv(0, 1);
     245                        Rinv(1, 1) = R(0, 0);
     246                        Rinv *= 1. / det;
    255247                }
    256                 InvReg(2, 2) = -Det;
    257                 //
    258                 InvReg *= 1.0 / h;
    259                 //
    260                 //
    261                 return InvReg.Similarity(D);
    262         }
     248        }
     249        //****************
     250        // case N > 2  ***
     251        //****************
    263252        else
    264253        {
    265                 std::cout << "RegInv3: found negative elements in diagonal. Return standard inversion." << std::endl;
    266                 return Smat.Invert();
    267         }
     254                // Break up matrix
     255                TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2);   // Upper left
     256                TVectorD p(N - 1);
     257                for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
     258                Double_t q = R(N - 1, N - 1);
     259                //Invert pieces and re-assemble
     260                TMatrixDSym Ainv(N - 1);
     261                TMatrixDSym A(N - 1);
     262                if (TMath::Abs(q) > 1.0e-15)
     263                {
     264                        // Case |q| > 0
     265                        Ainv.Rank1Update(p, -1.0 / q);
     266                        Ainv += Q;
     267                        A = RegInv(Ainv);               // Recursive call
     268                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
     269                        //
     270                        TVectorD b = (-1.0 / q) * (A * p);
     271                        for (Int_t i = 0; i < N - 1; i++)
     272                        {
     273                                Rinv(N - 1, i) = b(i);
     274                                Rinv(i, N - 1) = b(i);
     275                        }
     276                        //
     277                        Double_t pdotb = 0.;
     278                        for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
     279                        Double_t c = (1.0 - pdotb) / q;
     280                        Rinv(N - 1, N - 1) = c;
     281                }
     282                else
     283                {
     284                        // case q = 0
     285                        TMatrixDSym Qinv = RegInv(Q);           // Recursive call
     286                        Double_t a = Qinv.Similarity(p);
     287                        Double_t c = -1.0 / a;
     288                        Rinv(N - 1, N - 1) = c;
     289                        //
     290                        TVectorD b = (1.0 / a) * (Qinv * p);
     291                        for (Int_t i = 0; i < N - 1; i++)
     292                        {
     293                                Rinv(N - 1, i) = b(i);
     294                                Rinv(i, N - 1) = b(i);
     295                        }
     296                        //
     297                        A.Rank1Update(p, -1 / a);
     298                        A += Q;
     299                        A.Similarity(Qinv);
     300                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
     301                }
     302        }
     303        Minv = Rinv.Similarity(D);
     304        return Minv;
    268305}
    269306//
    … …  
    273310{
    274311        //
    275         // Derivative of track 3D position vector with respect to track parameters at constant phase
     312        // Derivative of track 3D position vector with respect to track parameters at constant phase 
    276313        //
    277314        // par = vector of track parameters
    … …  
    294331        A(2, 0) = 0.0;
    295332        // phi0
    296         A(0, 1) = -D*TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
    297         A(1, 1) = -D*TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
     333        A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
     334        A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    298335        A(2, 1) = 0.0;
    299336        // C
    300         A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C*C);
    301         A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C*C);
    302         A(2, 2) = -ct*phi / (2 * C*C);
     337        A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
     338        A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
     339        A(2, 2) = -ct * phi / (2 * C * C);
    303340        // z0
    304341        A(0, 3) = 0.0;
    … …  
    353390        Double_t ct = par(4);
    354391        //
    355         x0(0) = -D *TMath::Sin(p0);
    356         x0(1) = D*TMath::Cos(p0);
     392        x0(0) = -D * TMath::Sin(p0);
     393        x0(1) = D * TMath::Cos(p0);
    357394        x0(2) = z0;
    358395        //
    … …  
    378415        x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    379416        x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
    380         x(2) = x0(2) + ct*phi / (2 * C);
     417        x(2) = x0(2) + ct * phi / (2 * C);
    381418        //
    382419        return x;
    383420}
    384421//
    385 void  VertexFit::VertexFinder()
    386 {
    387         //
    388         // Vertex fit (units are meters)
     422void VertexFit::UpdateTrkArrays(Int_t i)
     423{
     424        //
     425        // Get track parameters and their covariance
     426        TVectorD par = *fPar[i];
     427        TMatrixDSym Cov = *fCov[i];
     428        //
     429        // Fill all track related work arrays arrays
     430        Double_t fs = ffi[i];                                           // Get phase
     431        TVectorD xs = Fill_x(par, fs);
     432        fx0i.push_back(new TVectorD(xs));                       // Start helix position
     433        //
     434        TMatrixD A = Fill_A(par, fs);                           // A = dx/da = derivatives wrt track parameters
     435        TMatrixDSym Winv = Cov.Similarity(A);           // W^-1 = A*C*A'
     436        fWinvi.push_back(new TMatrixDSym(Winv));                // Store W^-1 matrix
     437        //
     438        TMatrixDSym W = RegInv(Winv);                           // W = (A*C*A')^-1
     439        fWi.push_back(new TMatrixDSym(W));                      // Store W matrix
     440        //
     441        TVectorD a = Fill_a(par, fs);                           // a = dx/ds = derivatives wrt phase
     442        fai.push_back(new TVectorD(a));                         // Store a
     443        //
     444        Double_t a2 = W.Similarity(a);
     445        fa2i.push_back(a2);                                                     // Store a2
     446        //
     447        // Build D matrix
     448        TMatrixDSym B(3);
     449        B.Rank1Update(a, -1. / a2);
     450        B.Similarity(W);
     451        TMatrixDSym Ds = W + B;                                         // D matrix
     452        fDi.push_back(new TMatrixDSym(Ds));                     // Store D matrix
     453}
     454//
     455void  VertexFit::VertexFitter()
     456{
     457        //std::cout << "VertexFitter: just in" << std::endl;
     458        if (fNtr < 2)
     459        {
     460                std::cout << "VertexFit::VertexFitter - Method called with less than 2 tracks - Aborting " << std::endl;
     461                std::exit(1);
     462        }
     463        //
     464        // Vertex fit
    389465        //
    390466        // Initial variable definitions
    391467        TVectorD x(3);
    392468        TMatrixDSym covX(3);
    393         TVectorD x0(3); for (Int_t v = 0; v < 3; v++)x0(v) = 100.; // set to large value
    394469        Double_t Chi2 = 0;
    395         //
    396         // Stored quantities
    397         Double_t *fi = new Double_t[fNtr];                              // Phases
    398         TVectorD **x0i = new TVectorD*[fNtr];                   // Track expansion point
    399         TVectorD **ai = new TVectorD*[fNtr];                            // dx/dphi
    400         Double_t *a2i = new Double_t[fNtr];                             // a'Wa
    401         TMatrixDSym **Di = new TMatrixDSym*[fNtr];              // W-WBW
    402         TMatrixDSym **Wi = new TMatrixDSym*[fNtr];              // (ACA')^-1
    403         TMatrixDSym **Winvi = new TMatrixDSym*[fNtr];   // ACA'
    404         //
    405         // vertex radius approximation
    406         // Maximum impact parameter
    407         Double_t Rd = 0;
    408         for (Int_t i = 0; i < fNtr; i++)
    409         {
    410                 //ObsTrk* t = tracks[i];
    411                 TVectorD par = *fPar[i];
    412                 Double_t Dabs = TMath::Abs(par(0));
    413                 if (Dabs > Rd)Rd = Dabs;
    414         }
    415         //
    416         // Find track pair with largest phi difference
    417         Int_t isel=0; Int_t jsel=0; // selected track indices
    418         Double_t dphiMax = 0.0; // Max phi difference
    419         for (Int_t i = 0; i < fNtr-1; i++)
    420         {
    421                 //ObsTrk* ti = tracks[i];
    422                 TVectorD pari = *fPar[i];
    423                 Double_t phi1 = pari(1);
    424 
    425                 for (Int_t j = i+1; j < fNtr; j++)
    426                 {
    427                         //ObsTrk* tj = tracks[j];
    428                         TVectorD parj = *fPar[j];
    429                         Double_t phi2 = parj(1);
    430                         Double_t dphi = TMath::Abs(phi2 - phi1);
    431                         if (dphi > TMath::Pi())dphi = TMath::TwoPi() - dphi;
    432                         if (dphi > dphiMax)
    433                         {
    434                                 isel = i; jsel = j;
    435                                 dphiMax = dphi;
    436                         }
    437                 }
    438         }
    439         //
    440         //
    441         //ObsTrk* t1 = tracks[isel];
    442         TVectorD p1 = *fPar[isel];
    443         //ObsTrk* t2 = tracks[jsel];
    444         TVectorD p2 = *fPar[jsel];
    445         Double_t R = FastRv1(p1, p2);
    446         if (R > 1.0) R = Rd;
    447         R = 0.9*R + 0.1*Rd;
    448         //
    449         //std::cout << "VertexFinder: fast R = " << R << std::endl;
     470        TVectorD x0 = fXv;      // If previous fit done
     471        if (fRold < 0.0)for (Int_t i = 0; i < 3; i++)x0(i) = 1000.;     // Set to arbitrary large value if not
     472        //
     473        // Starting vertex radius approximation
     474        //
     475        Double_t R = fRold;                                             // Use previous fit if available
     476        if (R < 0.0) R = StartRadius();                 // Rough vertex estimate
    450477        //
    451478        // Iteration properties
    … …  
    456483        Double_t epsi = 1000.;
    457484        //
     485        // Iteration loop
    458486        while (epsi > eps && Ntry < TryMax)             // Iterate until found vertex is stable
    459487        {
     488                // Initialize arrays
    460489                x.Zero();
    461490                TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
    462                 covX.Zero();    // Reset vertex covariance
     491                covX.Zero();            // Reset vertex covariance
    463492                cterm.Zero();   // Reset constant term
    464493                H.Zero();               // Reset H matrix
    465494                DW1D.Zero();
    466495                //
    467                 //std::cout << "VertexFinder: start loop on tracks" << std::endl;
     496                // Reset work arrays
     497                //
     498                ResetWrkArrays();
     499                //
     500                // Start loop on tracks
     501                //
    468502                for (Int_t i = 0; i < fNtr; i++)
    469503                {
    470504                        // Get track helix parameters and their covariance matrix
    471                         //ObsTrk *t = tracks[i];
    472505                        TVectorD par = *fPar[i];
    473506                        TMatrixDSym Cov = *fCov[i];
     507                        //
     508                        // For first iteration only
    474509                        Double_t fs;
    475510                        if (Ntry <= 0)  // Initialize all phases on first pass
    … …  
    477512                                Double_t D = par(0);
    478513                                Double_t C = par(2);
    479                                 Double_t arg = TMath::Max(1.0e-6, (R*R - D*D) / (1 + 2 * C*D));
    480                                 fs = 2 * TMath::ASin(C*TMath::Sqrt(arg));
    481                                 fi[i] = fs;
     514                                Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
     515                                fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
     516                                ffi.push_back(fs);
    482517                        }
    483518                        //
    484                         // Starting values
    485                         //
    486                         fs = fi[i];                                                             // Get phase
    487                         //std::cout << "VertexFinder: phase fs set" << std::endl;
    488                         TVectorD xs = Fill_x(par, fs);
    489                         //std::cout << "VertexFinder: position xs set" << std::endl;
    490                         x0i[i] = new TVectorD(xs);                              // Start helix position
    491                         //std::cout << "VertexFinder: position x0i stored" << std::endl;
    492                         // W matrix = (A*C*A')^-1; W^-1 = A*C*A'
    493                         TMatrixD A = Fill_A(par, fs);                   // A = dx/da = derivatives wrt track parameters
    494                         //std::cout << "VertexFinder: derivatives A set" << std::endl;
    495                         TMatrixDSym Winv = Cov.Similarity(A);   // W^-1 = A*C*A'
    496                         Winvi[i] = new TMatrixDSym(Winv);               // Store W^-1 matrix
    497                         //std::cout << "VertexFinder: Winvi stored" << std::endl;
    498                         TMatrixDSym W = RegInv3(Winv);                  // W = (A*C*A')^-1
    499                         Wi[i] = new TMatrixDSym(W);                             // Store W matrix
    500                         //std::cout << "VertexFinder: Wi stored" << std::endl;
    501                         TVectorD a = Fill_a(par, fs);                   // a = dx/ds = derivatives wrt phase
    502                         //std::cout << "VertexFinder: derivatives a set" << std::endl;
    503                         ai[i] = new TVectorD(a);                                // Store a
    504                         //std::cout << "VertexFinder: derivatives a stored" << std::endl;
    505                         Double_t a2 = W.Similarity(a);
    506                         a2i[i] = a2;                                                    // Store a2
    507                         // Build D matrix
    508                         TMatrixDSym B(3);
    509                         B.Rank1Update(a, 1.0);
    510                         B *= -1. / a2;
    511                         B.Similarity(W);
    512                         TMatrixDSym Ds = W+B;                                   // D matrix
    513                         Di[i] = new TMatrixDSym(Ds);                    // Store D matrix
    514                         //std::cout << "VertexFinder: matrix Di stored" << std::endl;
     519                        // Update track related arrays
     520                        //
     521                        UpdateTrkArrays(i);
     522                        TMatrixDSym Ds = *fDi[i];
     523                        TMatrixDSym Winv = *fWinvi[i];
    515524                        TMatrixDSym DsW1Ds = Winv.Similarity(Ds);       // Service matrix to calculate covX
     525                        //
     526                        // Update global arrays
    516527                        DW1D += DsW1Ds;
    517528                        // Update hessian
    518529                        H += Ds;
    519530                        // update constant term
     531                        TVectorD xs = *fx0i[i];
    520532                        cterm += Ds * xs;
    521533                }                               // End loop on tracks
    522534                //
    523535                // update vertex position
    524                 TMatrixDSym H1 = RegInv3(H);
    525                 x = H1*cterm;
    526                 //std::cout << "VertexFinder: x vertex set" << std::endl;
     536                TMatrixDSym H1 = RegInv(H);
     537                x = H1 * cterm;
     538                //
    527539                // Update vertex covariance
    528540                covX = DW1D.Similarity(H1);
    529                 //std::cout << "VertexFinder: cov vertex set" << std::endl;
     541                //
    530542                // Update phases and chi^2
    531543                Chi2 = 0.0;
    532544                for (Int_t i = 0; i < fNtr; i++)
    533545                {
    534                         TVectorD lambda = (*Di[i])*(*x0i[i] - x);
    535                         TMatrixDSym Wm1 = *Winvi[i];
     546                        TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
     547                        TMatrixDSym Wm1 = *fWinvi[i];
    536548                        fChi2List(i) = Wm1.Similarity(lambda);
    537549                        Chi2 += fChi2List(i);
    538                         TVectorD a = *ai[i];
    539                         TVectorD b = (*Wi[i])*(x - *x0i[i]);
    540                         for (Int_t j = 0; j < 3; j++)fi[i] += a(j)*b(j) / a2i[i];
     550                        TVectorD a = *fai[i];
     551                        TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));
     552                        for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
    541553                }
    542 
    543                 //std::cout << "VertexFinder: end chi2 calculation" << std::endl;
    544554                //
    545555                TVectorD dx = x - x0;
    546556                x0 = x;
    547557                // update vertex stability
    548                 TMatrixDSym Hess = RegInv3(covX);
     558                TMatrixDSym Hess = RegInv(covX);
    549559                epsi = Hess.Similarity(dx);
    550560                Ntry++;
    … …  
    552562                // Store result
    553563                //
    554                 fXv = x;                        // Vertex position
     564                fXv = x;                                // Vertex position
    555565                fcovXv = covX;          // Vertex covariance
    556566                fChi2 = Chi2;           // Vertex fit Chi2
    557                 //
    558                 // Store intermediate data
    559                 //
    560 
    561                 //std::cout << "VertexFinder: before store intermediate data" << std::endl;
    562                 for (Int_t i = 0; i < fNtr; i++)
    563                 {
    564                         //std::cout << "VertexFinder: inside store intermediate data" << std::endl;
    565                         //std::cout << "i = " << i << ", fi[i] = " << fi[i] << std::endl;
    566                         //std::cout << "i = " << i << ", ffi[i] = " << ffi[i] << std::endl;
    567                         ffi[i] = fi[i];                         // Fit phases
    568                         //std::cout << "VertexFinder: fi stored" << std::endl;
    569                         fx0i[i] = x0i[i];                       // Track expansion points
    570                         //std::cout << "VertexFinder: x0i stored" << std::endl;
    571                         fai[i] = ai[i];                         // dx/dphi
    572                         //std::cout << "VertexFinder: ai stored" << std::endl;
    573                         fa2i[i] = a2i[i];                       // a'Wa
    574                         //std::cout << "VertexFinder: a2i stored" << std::endl;
    575                         fDi[i] = Di[i];                         // W-WBW
    576                         //std::cout << "VertexFinder: Di stored" << std::endl;
    577                         fWi[i] = Wi[i];                         // (ACA')^-1
    578                         //std::cout << "VertexFinder: Wi stored" << std::endl;
    579                         fWinvi[i] = Winvi[i];           // ACA'
    580                         //std::cout << "VertexFinder: Winvi stored" << std::endl;
    581                 }
    582                 //std::cout << "Iteration " << Ntry << " completed - Before cleanup" << std::endl;
    583                 //
    584                 // Cleanup
    585                 //
    586                 for (Int_t i = 0; i < fNtr; i++)
    587                 {
    588                         x0i[i]->Clear();
    589                         Winvi[i]->Clear();
    590                         Wi[i]->Clear();
    591                         ai[i]->Clear();
    592                         Di[i]->Clear();
    593 
    594                         delete x0i[i];
    595                         delete Winvi[i];
    596                         delete Wi[i];
    597                         delete ai[i];
    598                         delete Di[i];
    599                 }
    600 
    601                 //std::cout << "Iteration " << Ntry << " completed - After cleanup" << std::endl;
    602         }
    603         //
    604         fVtxDone = kTRUE;       // Set fitting completion flag
    605         //
    606         delete[] fi;            // Phases
    607         delete[] x0i;           // Track expansion point
    608         delete[] ai;            // dx/dphi
    609         delete[] a2i;           // a'Wa
    610         delete[] Di;            // W-WBW
    611         delete[] Wi;            // (ACA')^-1
    612         delete[] Winvi;         // ACA'
    613 }
    614 //
     567        }               // end of iteration loop
     568        //
     569        fVtxDone = kTRUE;               // Set fit completion flag
     570        fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1));     // Store fit radius
     571        //
     572}
     573//
     574// Return fit vertex
    615575TVectorD VertexFit::GetVtx()
    616576{
    617         //std::cout << "GetVtx: flag set to " << fVtxDone << std::endl;
    618         if (!fVtxDone)VertexFinder();
     577        if (!fVtxDone) VertexFitter();
    619578        return fXv;
    620579}
    621 
     580//
     581// Return fit vertex covariance
    622582TMatrixDSym VertexFit::GetVtxCov()
    623583{
    624         if (!fVtxDone)VertexFinder();
     584        if (!fVtxDone) VertexFitter();
    625585        return fcovXv;
    626586}
    627 
     587//
     588// Return fit vertex chi2
    628589Double_t VertexFit::GetVtxChi2()
    629590{
    630         if (!fVtxDone)VertexFinder();
     591        if (!fVtxDone) VertexFitter();
    631592        return fChi2;
    632593}
    633 
     594//
     595// Return array of chi2 contributions from each track
    634596TVectorD VertexFit::GetVtxChi2List()
    635597{
    636         if (!fVtxDone)VertexFinder();
     598        if (!fVtxDone) VertexFitter();
    637599        return fChi2List;
    638600}
    … …  
    644606}
    645607//
    646 void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov)                   // Add track to input list
    647 {
    648         std::cout << "VertexFit::AddTrk: Not implemented yet" << std::endl;
    649 }
    650 void VertexFit::RemoveTrk(Int_t iTrk)                                                   // Remove iTrk track
    651 {
    652         std::cout << "VertexFit::RemoveTrk: Not implemented yet" << std::endl;
    653 }
     608// Adding tracks one by one
     609void VertexFit::AddTrk(TVectorD *par, TMatrixDSym *Cov)                 // Add track to input list
     610{
     611        fNtr++;
     612        fChi2List.ResizeTo(fNtr);       // Resize chi2 array
     613        fPar.push_back(par);                    // add new track
     614        fCov.push_back(Cov);
     615        //
     616        // Reset previous vertex temp arrays
     617        ResetWrkArrays();
     618        ffi.clear();
     619        fVtxDone = kFALSE;                      // Reset vertex done flag
     620}
     621//
     622// Removing tracks one by one
     623void VertexFit::RemoveTrk(Int_t iTrk)   // Remove iTrk track
     624{
     625        fNtr--;
     626        fChi2List.Clear();
     627        fChi2List.ResizeTo(fNtr);               // Resize chi2 array
     628        fPar.erase(fPar.begin() + iTrk);                // Remove track
     629        fCov.erase(fCov.begin() + iTrk);
     630        //
     631        // Reset previous vertex temp arrays
     632        ResetWrkArrays();
     633        ffi.clear();
     634        fVtxDone = kFALSE;                      // Reset vertex done flag
     635}
Note: See TracChangeset for help on using the changeset viewer.