Fork me on GitHub

Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/VertexFit.cc

    r82db145 r127644a  
    88// Constructors
    99//
    10 //
    11 // Empty construction (to be used when adding tracks later with AddTrk() )
     10// Empty
    1211VertexFit::VertexFit()
    1312{
    1413        fNtr = 0;
    15         fRold = -1.0;
    1614        fVtxDone = kFALSE;
    1715        fVtxCst = kFALSE;
    1816        fxCst.ResizeTo(3);
    19         fCovCst.ResizeTo(3, 3);
     17        fCovCst.ResizeTo(3,3);
    2018        fXv.ResizeTo(3);
    21         fcovXv.ResizeTo(3, 3);
    22 }
    23 //
    24 // Build from list of parameters and covariances
     19        fcovXv.ResizeTo(3,3);
     20}
     21// Parameters and covariances
    2522VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
    2623{
    2724        fNtr = Ntr;
    28         fRold = -1.0;
    2925        fVtxDone = kFALSE;
    3026        fVtxCst = kFALSE;
    3127        fxCst.ResizeTo(3);
    32         fCovCst.ResizeTo(3, 3);
     28        fCovCst.ResizeTo(3,3);
    3329        fXv.ResizeTo(3);
    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
     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
    4851VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
    4952{
    5053        fNtr = Ntr;
    51         fRold = -1.0;
    5254        fVtxDone = kFALSE;
    5355        fVtxCst = kFALSE;
    5456        fxCst.ResizeTo(3);
    55         fCovCst.ResizeTo(3, 3);
     57        fCovCst.ResizeTo(3,3);
    5658        fXv.ResizeTo(3);
    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         }
     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;
    6586}
    6687//
    6788// Destructor
    68 //
    69 void 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 }
    8789VertexFit::~VertexFit()
    8890{
    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();
     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        }
    105108        fNtr = 0;
    106 }
    107 //
    108 Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
    109 {
    110         //
    111         // Find radius of minimum distance between two tracks
     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//
     120Double_t VertexFit::FastRv1(TVectorD p1, TVectorD p2)
     121{
     122        //
     123        // Find radius of intersection between two tracks in the transverse plane
    112124        //
    113125        // p = (D,phi, C, z0, ct)
    … …  
    115127        // Define arrays
    116128        //
    117         Double_t C1 = p1(2);
    118         Double_t C2 = p2(2);
    119         Double_t ph1 = p1(1);
    120         Double_t ph2 = p2(1);
     129        Double_t r1 = 1.0 / p1(2);
     130        Double_t r2 = 1.0 / p2(2);
    121131        TVectorD x0 = Fill_x0(p1);
    122132        TVectorD y0 = Fill_x0(p2);
    123133        TVectorD n = Fill_a(p1, 0.0);
    124         n *= (2*C1);
     134        n *= r1;
    125135        TVectorD k = Fill_a(p2, 0.0);
    126         k *= (2*C2);
     136        k *= r2;
    127137        //
    128138        // Setup and solve linear system
    … …  
    138148        H(1, 1) = nn;
    139149        TVectorD c(2);
    140         c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
     150        c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) +=  n(i) * (y0(i) - x0(i));
    141151        c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
    142152        TVectorD smin = (H * c);
    … …  
    145155        TVectorD X = x0 + smin(0) * n;
    146156        TVectorD Y = y0 + smin(1) * k;
    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
    161 Double_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++)
    183                 {
    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                         }
     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}
     162Double_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)
     196                {
     197                        std::cout << "FastRv: maximum number of iteration reached" << std::endl;
     198                        break;
    192199                }
    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
     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        }
    203206        //
    204207        return R;
    205208}
    206 //
    207 // Regularized symmetric matrix inversion
    208 //
    209 TMatrixDSym 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)
    235                 {
    236                         std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
    237                         Rinv.Zero();    // Return null matrix
     209
     210TMatrixDSym 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++)
     231                {
     232                        for (Int_t j = 0; j < 2; j++)Q(i, j) = RegMat(i, j);
    238233                }
    239                 else
    240                 {
    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;
     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++)
     251                {
     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);
    247255                }
    248         }
    249         //****************
    250         // case N > 2  ***
    251         //****************
     256                InvReg(2, 2) = -Det;
     257                //
     258                InvReg *= 1.0 / h;
     259                //
     260                //
     261                return InvReg.Similarity(D);
     262        }
    252263        else
    253264        {
    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;
     265                std::cout << "RegInv3: found negative elements in diagonal. Return standard inversion." << std::endl;
     266                return Smat.Invert();
     267        }
    305268}
    306269//
    … …  
    310273{
    311274        //
    312         // Derivative of track 3D position vector with respect to track parameters at constant phase 
     275        // Derivative of track 3D position vector with respect to track parameters at constant phase
    313276        //
    314277        // par = vector of track parameters
    … …  
    331294        A(2, 0) = 0.0;
    332295        // phi0
    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);
     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);
    335298        A(2, 1) = 0.0;
    336299        // 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);
     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);
    340303        // z0
    341304        A(0, 3) = 0.0;
    … …  
    390353        Double_t ct = par(4);
    391354        //
    392         x0(0) = -D * TMath::Sin(p0);
    393         x0(1) = D * TMath::Cos(p0);
     355        x0(0) = -D *TMath::Sin(p0);
     356        x0(1) = D*TMath::Cos(p0);
    394357        x0(2) = z0;
    395358        //
    … …  
    415378        x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    416379        x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
    417         x(2) = x0(2) + ct * phi / (2 * C);
     380        x(2) = x0(2) + ct*phi / (2 * C);
    418381        //
    419382        return x;
    420383}
    421384//
    422 void 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 //
    455 void  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
     385void  VertexFit::VertexFinder()
     386{
     387        //
     388        // Vertex fit (units are meters)
    465389        //
    466390        // Initial variable definitions
    467391        TVectorD x(3);
    468392        TMatrixDSym covX(3);
     393        TVectorD x0(3); for (Int_t v = 0; v < 3; v++)x0(v) = 100.; // set to large value
    469394        Double_t Chi2 = 0;
    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
     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;
    477450        //
    478451        // Iteration properties
    … …  
    483456        Double_t epsi = 1000.;
    484457        //
    485         // Iteration loop
    486458        while (epsi > eps && Ntry < TryMax)             // Iterate until found vertex is stable
    487459        {
    488                 // Initialize arrays
    489460                x.Zero();
    490461                TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
    491                 covX.Zero();            // Reset vertex covariance
     462                covX.Zero();    // Reset vertex covariance
    492463                cterm.Zero();   // Reset constant term
    493464                H.Zero();               // Reset H matrix
    494465                DW1D.Zero();
    495466                //
    496                 // Reset work arrays
    497                 //
    498                 ResetWrkArrays();
    499                 //
    500                 // Start loop on tracks
    501                 //
     467                //std::cout << "VertexFinder: start loop on tracks" << std::endl;
    502468                for (Int_t i = 0; i < fNtr; i++)
    503469                {
    504470                        // Get track helix parameters and their covariance matrix
     471                        //ObsTrk *t = tracks[i];
    505472                        TVectorD par = *fPar[i];
    506473                        TMatrixDSym Cov = *fCov[i];
    507                         //
    508                         // For first iteration only
    509474                        Double_t fs;
    510475                        if (Ntry <= 0)  // Initialize all phases on first pass
    … …  
    512477                                Double_t D = par(0);
    513478                                Double_t C = par(2);
    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);
     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;
    517482                        }
    518483                        //
    519                         // Update track related arrays
     484                        // Starting values
    520485                        //
    521                         UpdateTrkArrays(i);
    522                         TMatrixDSym Ds = *fDi[i];
    523                         TMatrixDSym Winv = *fWinvi[i];
     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;
    524515                        TMatrixDSym DsW1Ds = Winv.Similarity(Ds);       // Service matrix to calculate covX
    525                         //
    526                         // Update global arrays
    527516                        DW1D += DsW1Ds;
    528517                        // Update hessian
    529518                        H += Ds;
    530519                        // update constant term
    531                         TVectorD xs = *fx0i[i];
    532520                        cterm += Ds * xs;
    533521                }                               // End loop on tracks
    534522                //
    535523                // update vertex position
    536                 TMatrixDSym H1 = RegInv(H);
    537                 x = H1 * cterm;
    538                 //
     524                TMatrixDSym H1 = RegInv3(H);
     525                x = H1*cterm;
     526                //std::cout << "VertexFinder: x vertex set" << std::endl;
    539527                // Update vertex covariance
    540528                covX = DW1D.Similarity(H1);
    541                 //
     529                //std::cout << "VertexFinder: cov vertex set" << std::endl;
    542530                // Update phases and chi^2
    543531                Chi2 = 0.0;
    544532                for (Int_t i = 0; i < fNtr; i++)
    545533                {
    546                         TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
    547                         TMatrixDSym Wm1 = *fWinvi[i];
     534                        TVectorD lambda = (*Di[i])*(*x0i[i] - x);
     535                        TMatrixDSym Wm1 = *Winvi[i];
    548536                        fChi2List(i) = Wm1.Similarity(lambda);
    549537                        Chi2 += fChi2List(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];
     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];
    553541                }
     542
     543                //std::cout << "VertexFinder: end chi2 calculation" << std::endl;
    554544                //
    555545                TVectorD dx = x - x0;
    556546                x0 = x;
    557547                // update vertex stability
    558                 TMatrixDSym Hess = RegInv(covX);
     548                TMatrixDSym Hess = RegInv3(covX);
    559549                epsi = Hess.Similarity(dx);
    560550                Ntry++;
    … …  
    562552                // Store result
    563553                //
    564                 fXv = x;                                // Vertex position
     554                fXv = x;                        // Vertex position
    565555                fcovXv = covX;          // Vertex covariance
    566556                fChi2 = Chi2;           // Vertex fit Chi2
    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
     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//
    575615TVectorD VertexFit::GetVtx()
    576616{
    577         if (!fVtxDone) VertexFitter();
     617        //std::cout << "GetVtx: flag set to " << fVtxDone << std::endl;
     618        if (!fVtxDone)VertexFinder();
    578619        return fXv;
    579620}
    580 //
    581 // Return fit vertex covariance
     621
    582622TMatrixDSym VertexFit::GetVtxCov()
    583623{
    584         if (!fVtxDone) VertexFitter();
     624        if (!fVtxDone)VertexFinder();
    585625        return fcovXv;
    586626}
    587 //
    588 // Return fit vertex chi2
     627
    589628Double_t VertexFit::GetVtxChi2()
    590629{
    591         if (!fVtxDone) VertexFitter();
     630        if (!fVtxDone)VertexFinder();
    592631        return fChi2;
    593632}
    594 //
    595 // Return array of chi2 contributions from each track
     633
    596634TVectorD VertexFit::GetVtxChi2List()
    597635{
    598         if (!fVtxDone) VertexFitter();
     636        if (!fVtxDone)VertexFinder();
    599637        return fChi2List;
    600638}
    … …  
    606644}
    607645//
    608 // Adding tracks one by one
    609 void 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
    623 void 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 }
     646void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov)                   // Add track to input list
     647{
     648        std::cout << "VertexFit::AddTrk: Not implemented yet" << std::endl;
     649}
     650void VertexFit::RemoveTrk(Int_t iTrk)                                                   // Remove iTrk track
     651{
     652        std::cout << "VertexFit::RemoveTrk: Not implemented yet" << std::endl;
     653}
Note: See TracChangeset for help on using the changeset viewer.