Fork me on GitHub

Ignore:
Timestamp:
Mar 7, 2021, 9:48:26 AM (4 years ago)
Author:
Franco BEDESCHI <bed@…>
Branches:
master
Children:
bd33fce
Parents:
0d65492 (diff), 363e269 (diff)
Note: this is a merge changeset, the changes displayed below correspond to the merge itself.
Use the (diff) links above to see all the changes relative to each parent.
Message:

Fixed conflicts and Reginv3

File:
1 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/VertexFit.cc

    r0d65492 r91ef0b8  
    1515        fVtxCst = kFALSE;
    1616        fxCst.ResizeTo(3);
    17         fCovCst.ResizeTo(3,3);
     17        fCovCst.ResizeTo(3, 3);
    1818        fXv.ResizeTo(3);
    19         fcovXv.ResizeTo(3,3);
     19        fcovXv.ResizeTo(3, 3);
    2020}
    2121// Parameters and covariances
    … …  
    2626        fVtxCst = kFALSE;
    2727        fxCst.ResizeTo(3);
    28         fCovCst.ResizeTo(3,3);
     28        fCovCst.ResizeTo(3, 3);
    2929        fXv.ResizeTo(3);
    30         fcovXv.ResizeTo(3,3);
     30        fcovXv.ResizeTo(3, 3);
    3131        //
    3232        fPar = trkPar;
    … …  
    3535        //
    3636        ffi = new Double_t[Ntr];                                // Fit phases
    37         fx0i = new TVectorD* [Ntr];                     // Track expansion points
     37        fx0i = new TVectorD * [Ntr];                    // Track expansion points
    3838        for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
    3939        fai = new TVectorD * [Ntr];                     // dx/dphi
    4040        for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
    4141        fa2i = new Double_t[Ntr];                               // a'Wa
    42         fDi = new TMatrixDSym*[Ntr];            // W-WBW
     42        fDi = new TMatrixDSym * [Ntr];          // W-WBW
    4343        for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
    4444        fWi = new TMatrixDSym * [Ntr];  // (ACA')^-1
    … …  
    4646        fWinvi = new TMatrixDSym * [Ntr];       // ACA'
    4747        for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
    48        
    4948}
    5049// ObsTrk list
    … …  
    5554        fVtxCst = kFALSE;
    5655        fxCst.ResizeTo(3);
    57         fCovCst.ResizeTo(3,3);
     56        fCovCst.ResizeTo(3, 3);
    5857        fXv.ResizeTo(3);
    59         fcovXv.ResizeTo(3,3);
    60         //
    61         fPar = new TVectorD*[Ntr];
    62         fCov = new TMatrixDSym*[Ntr];
     58        fcovXv.ResizeTo(3, 3);
     59        //
     60        fPar = new TVectorD * [Ntr];
     61        fCov = new TMatrixDSym * [Ntr];
    6362        fChi2List.ResizeTo(Ntr);
    6463        for (Int_t i = 0; i < Ntr; i++)
    6564        {
    6665                fPar[i] = new TVectorD(track[i]->GetObsPar());
    67                 //cout << "fPar = " << endl; fPar[i]->Print();
    6866                fCov[i] = new TMatrixDSym(track[i]->GetCov());
    69                 //cout << "fCov = " << endl; fCov[i]->Print();
    7067        }
    7168        //
    … …  
    8279        fWinvi = new TMatrixDSym * [Ntr];       // ACA'
    8380        for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
    84 
    85         //cout << "VertexFit constructor executed" << endl;
    8681}
    8782//
    … …  
    8984VertexFit::~VertexFit()
    9085{
    91         fxCst.Clear(); 
     86        fxCst.Clear();
    9287        fCovCst.Clear();
    93         fXv.Clear();   
    94         fcovXv.Clear(); 
     88        fXv.Clear();
     89        fcovXv.Clear();
    9590        fChi2List.Clear();
    9691        //
    97         for (Int_t i= 0; i < fNtr; i++)
    98         {
    99                 fPar[i]->Clear(); 
     92        for (Int_t i = 0; i < fNtr; i++)
     93        {
     94                fPar[i]->Clear();
    10095                fCov[i]->Clear();
    10196                //
    … …  
    148143        H(1, 1) = nn;
    149144        TVectorD c(2);
    150         c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) +=  n(i) * (y0(i) - x0(i));
     145        c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
    151146        c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
    152147        TVectorD smin = (H * c);
    … …  
    158153        Double_t R2 = TMath::Sqrt(Y(0) * Y(0) + Y(1) * Y(1));
    159154        //
    160         return 0.5*(R1+R2);
     155        return 0.5 * (R1 + R2);
    161156}
    162157Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
    … …  
    170165        TMatrixDSym H(2);
    171166        H(0, 0) = -TMath::Cos(p2(1));
    172         H(0, 1) =  TMath::Cos(p1(1));
     167        H(0, 1) = TMath::Cos(p1(1));
    173168        H(1, 0) = -TMath::Sin(p2(1));
    174         H(1, 1) =  TMath::Sin(p1(1));
     169        H(1, 1) = TMath::Sin(p1(1));
    175170        Double_t Det = TMath::Sin(p2(1) - p1(1));
    176171        H *= 1.0 / Det;
    … …  
    195190                if (Ntry > NtryMax)
    196191                {
    197                         cout << "FastRv: maximum number of iteration reached" << endl;
     192                        std::cout << "FastRv: maximum number of iteration reached" << std::endl;
    198193                        break;
    199194                }
    … …  
    208203}
    209204
    210 TMatrixDSym VertexFit::RegInv3(TMatrixDSym &Smat0)
     205TMatrixDSym VertexFit::RegInv3(TMatrixDSym& Smat0)
    211206{
    212207        //
    … …  
    217212        if (N != 3)
    218213        {
    219                 cout << "RegInv3 called with  matrix size != 3. Abort & return standard inversion." << endl;
     214                std::cout << "RegInv3 called with  matrix size != 3. Abort & return standard inversion." << std::endl;
    220215                return Smat.Invert();
    221216        }
    … …  
    232227                        for (Int_t j = 0; j < 2; j++)Q(i, j) = RegMat(i, j);
    233228                }
    234                 Double_t Det = 1 - Q(0, 1)*Q(1, 0);
     229                Double_t Det = 1 - Q(0, 1) * Q(1, 0);
    235230                TMatrixDSym H(2);
    236231                H = Q;
    … …  
    241236                p(1) = RegMat(1, 2);
    242237                Double_t pHp = H.Similarity(p);
    243                 Double_t h = pHp-Det;
     238                Double_t h = pHp - Det;
    244239                //
    245240                TMatrixDSym pp(2); pp.Rank1Update(p);
    246                 TMatrixDSym F = (h*H) - pp.Similarity(H);
     241                TMatrixDSym F = (h * H) - pp.Similarity(H);
    247242                F *= 1.0 / Det;
    248                 TVectorD b = H*p;
     243                TVectorD b = H * p;
    249244                TMatrixDSym InvReg(3);
    250245                for (Int_t i = 0; i < 2; i++)
    … …  
    263258        else
    264259        {
    265                 cout << "RegInv3: found negative elements in diagonal. Return standard inversion." << endl;
    266                 return Smat.Invert();
     260                D.Zero();
     261                for (Int_t i = 0; i < N; i++) D(i, i) = 1.0 / TMath::Sqrt(TMath::Abs(Smat(i, i)));
     262                TMatrixDSym RegMat = Smat.Similarity(D);
     263                RegMat.Invert();
     264                return RegMat.Similarity(D);
    267265        }
    268266}
    … …  
    294292        A(2, 0) = 0.0;
    295293        // 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);
     294        A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
     295        A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    298296        A(2, 1) = 0.0;
    299297        // 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);
     298        A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
     299        A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
     300        A(2, 2) = -ct * phi / (2 * C * C);
    303301        // z0
    304302        A(0, 3) = 0.0;
    … …  
    353351        Double_t ct = par(4);
    354352        //
    355         x0(0) = -D *TMath::Sin(p0);
    356         x0(1) = D*TMath::Cos(p0);
     353        x0(0) = -D * TMath::Sin(p0);
     354        x0(1) = D * TMath::Cos(p0);
    357355        x0(2) = z0;
    358356        //
    … …  
    378376        x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
    379377        x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
    380         x(2) = x0(2) + ct*phi / (2 * C);
     378        x(2) = x0(2) + ct * phi / (2 * C);
    381379        //
    382380        return x;
    … …  
    395393        //
    396394        // 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'
     395        Double_t* fi = new Double_t[fNtr];                              // Phases
     396        TVectorD** x0i = new TVectorD * [fNtr];                 // Track expansion point
     397        TVectorD** ai = new TVectorD * [fNtr];                          // dx/dphi
     398        Double_t* a2i = new Double_t[fNtr];                             // a'Wa
     399        TMatrixDSym** Di = new TMatrixDSym * [fNtr];            // W-WBW
     400        TMatrixDSym** Wi = new TMatrixDSym * [fNtr];            // (ACA')^-1
     401        TMatrixDSym** Winvi = new TMatrixDSym * [fNtr]; // ACA'
    404402        //
    405403        // vertex radius approximation
    … …  
    415413        //
    416414        // 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++)
     415        Int_t isel = 0; Int_t jsel = 0; // selected track indices
     416        Double_t dphiMax = 0.0; // Max phi difference
     417
     418        for (Int_t i = 0; i < fNtr - 1; i++)
    420419        {
    421420                //ObsTrk* ti = tracks[i];
    … …  
    423422                Double_t phi1 = pari(1);
    424423
    425                 for (Int_t j = i+1; j < fNtr; j++)
     424                for (Int_t j = i + 1; j < fNtr; j++)
    426425                {
    427426                        //ObsTrk* tj = tracks[j];
    … …  
    438437        }
    439438        //
    440         //
    441         //ObsTrk* t1 = tracks[isel];
    442439        TVectorD p1 = *fPar[isel];
    443         //ObsTrk* t2 = tracks[jsel];
    444440        TVectorD p2 = *fPar[jsel];
    445441        Double_t R = FastRv1(p1, p2);
    446         if (R > 1.0) R = Rd;
    447         R = 0.9*R + 0.1*Rd;
    448         //
    449         //cout << "VertexFinder: fast R = " << R << endl;
     442        if (R > 1000.0) R = Rd;
     443        R = 0.9 * R + 0.1 * Rd;
    450444        //
    451445        // Iteration properties
    … …  
    464458                H.Zero();               // Reset H matrix
    465459                DW1D.Zero();
    466                 // 
    467                 //cout << "VertexFinder: start loop on tracks" << endl;
     460                //
     461                //std::cout << "VertexFinder: start loop on tracks" << std::endl;
    468462                for (Int_t i = 0; i < fNtr; i++)
    469463                {
    470                         // Get track helix parameters and their covariance matrix
    471                         //ObsTrk *t = tracks[i];
     464                        // Get track helix parameters and their covariance matrix
    472465                        TVectorD par = *fPar[i];
    473                         TMatrixDSym Cov = *fCov[i];
     466                        TMatrixDSym Cov = *fCov[i];
     467
    474468                        Double_t fs;
    475469                        if (Ntry <= 0)  // Initialize all phases on first pass
    … …  
    477471                                Double_t D = par(0);
    478472                                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));
     473                                Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
     474                                fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
    481475                                fi[i] = fs;
    482476                        }
    … …  
    484478                        // Starting values
    485479                        //
    486                         fs = fi[i];                                                             // Get phase
    487                         //cout << "VertexFinder: phase fs set" << endl;
     480                        fs = fi[i];             // Get phase
     481                        //std::cout << "VertexFinder: phase fs set" << std::endl;
    488482                        TVectorD xs = Fill_x(par, fs);
    489                         //cout << "VertexFinder: position xs set" << endl;
     483                        //std::cout << "VertexFinder: position xs set" << std::endl;
    490484                        x0i[i] = new TVectorD(xs);                              // Start helix position
    491                         //cout << "VertexFinder: position x0i stored" << endl;
     485                        //std::cout << "VertexFinder: position x0i stored" << std::endl;
    492486                        // W matrix = (A*C*A')^-1; W^-1 = A*C*A'
    493487                        TMatrixD A = Fill_A(par, fs);                   // A = dx/da = derivatives wrt track parameters
    494                         //cout << "VertexFinder: derivatives A set" << endl;
     488                        //std::cout << "VertexFinder: derivatives A set" << std::endl;
    495489                        TMatrixDSym Winv = Cov.Similarity(A);   // W^-1 = A*C*A'
    496490                        Winvi[i] = new TMatrixDSym(Winv);               // Store W^-1 matrix
    497                         //cout << "VertexFinder: Winvi stored" << endl;
     491                        //std::cout << "VertexFinder: Winvi stored" << std::endl;
    498492                        TMatrixDSym W = RegInv3(Winv);                  // W = (A*C*A')^-1
    499493                        Wi[i] = new TMatrixDSym(W);                             // Store W matrix
    500                         //cout << "VertexFinder: Wi stored" << endl;
     494                        //std::cout << "VertexFinder: Wi stored" << std::endl;
    501495                        TVectorD a = Fill_a(par, fs);                   // a = dx/ds = derivatives wrt phase
    502                         //cout << "VertexFinder: derivatives a set" << endl;
     496                        //std::cout << "VertexFinder: derivatives a set" << std::endl;
    503497                        ai[i] = new TVectorD(a);                                // Store a
    504                         //cout << "VertexFinder: derivatives a stored" << endl;
     498                        //std::cout << "VertexFinder: derivatives a stored" << std::endl;
    505499                        Double_t a2 = W.Similarity(a);
    506500                        a2i[i] = a2;                                                    // Store a2
    507501                        // Build D matrix
    508                         TMatrixDSym B(3);
     502                        TMatrixDSym B(3);
     503
    509504                        B.Rank1Update(a, 1.0);
    510505                        B *= -1. / a2;
    511506                        B.Similarity(W);
    512                         TMatrixDSym Ds = W+B;                                   // D matrix
     507                        TMatrixDSym Ds = W + B;                                 // D matrix
    513508                        Di[i] = new TMatrixDSym(Ds);                    // Store D matrix
    514                         //cout << "VertexFinder: matrix Di stored" << endl;
     509                        //std::cout << "VertexFinder: matrix Di stored" << std::endl;
    515510                        TMatrixDSym DsW1Ds = Winv.Similarity(Ds);       // Service matrix to calculate covX
    516                         DW1D += DsW1Ds;                                                         
     511                        DW1D += DsW1Ds;
    517512                        // Update hessian
    518513                        H += Ds;
    … …  
    523518                // update vertex position
    524519                TMatrixDSym H1 = RegInv3(H);
    525                 x = H1*cterm;
    526                 //cout << "VertexFinder: x vertex set" << endl;
     520                x = H1 * cterm;
     521                //std::cout << "VertexFinder: x vertex set" << std::endl;
    527522                // Update vertex covariance
    528523                covX = DW1D.Similarity(H1);
    529                 //cout << "VertexFinder: cov vertex set" << endl;
     524                //std::cout << "VertexFinder: cov vertex set" << std::endl;
    530525                // Update phases and chi^2
    531526                Chi2 = 0.0;
    532527                for (Int_t i = 0; i < fNtr; i++)
    533528                {
    534                         TVectorD lambda = (*Di[i])*(*x0i[i] - x);
     529                        TVectorD lambda = (*Di[i]) * (*x0i[i] - x);
    535530                        TMatrixDSym Wm1 = *Winvi[i];
    536531                        fChi2List(i) = Wm1.Similarity(lambda);
    537532                        Chi2 += fChi2List(i);
    538533                        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];
     534                        TVectorD b = (*Wi[i]) * (x - *x0i[i]);
     535                        for (Int_t j = 0; j < 3; j++)fi[i] += a(j) * b(j) / a2i[i];
    541536                }
    542537
    543                 //cout << "VertexFinder: end chi2 calculation" << endl;
    544538                //
    545539                TVectorD dx = x - x0;
    … …  
    559553                //
    560554
    561                 //cout << "VertexFinder: before store intermediate data" << endl;
     555                //std::cout << "VertexFinder: before store intermediate data" << std::endl;
    562556                for (Int_t i = 0; i < fNtr; i++)
    563557                {
    564                         //cout << "VertexFinder: inside store intermediate data" << endl;
    565                         //cout << "i = " << i << ", fi[i] = " << fi[i] << endl;
    566                         //cout << "i = " << i << ", ffi[i] = " << ffi[i] << endl;
     558                        //std::cout << "VertexFinder: inside store intermediate data" << std::endl;
     559                        //std::cout << "i = " << i << ", fi[i] = " << fi[i] << std::endl;
     560                        //std::cout << "i = " << i << ", ffi[i] = " << ffi[i] << std::endl;
    567561                        ffi[i] = fi[i];                         // Fit phases
    568                         //cout << "VertexFinder: fi stored" << endl;
     562                        //std::cout << "VertexFinder: fi stored" << std::endl;
    569563                        fx0i[i] = x0i[i];                       // Track expansion points
    570                         //cout << "VertexFinder: x0i stored" << endl;
     564                        //std::cout << "VertexFinder: x0i stored" << std::endl;
    571565                        fai[i] = ai[i];                         // dx/dphi
    572                         //cout << "VertexFinder: ai stored" << endl;
     566                        //std::cout << "VertexFinder: ai stored" << std::endl;
    573567                        fa2i[i] = a2i[i];                       // a'Wa
    574                         //cout << "VertexFinder: a2i stored" << endl;
     568                        //std::cout << "VertexFinder: a2i stored" << std::endl;
    575569                        fDi[i] = Di[i];                         // W-WBW
    576                         //cout << "VertexFinder: Di stored" << endl;
     570                        //std::cout << "VertexFinder: Di stored" << std::endl;
    577571                        fWi[i] = Wi[i];                         // (ACA')^-1
    578                         //cout << "VertexFinder: Wi stored" << endl;
     572                        //std::cout << "VertexFinder: Wi stored" << std::endl;
    579573                        fWinvi[i] = Winvi[i];           // ACA'
    580                         //cout << "VertexFinder: Winvi stored" << endl;
     574                        //std::cout << "VertexFinder: Winvi stored" << std::endl;
    581575                }
    582                 //cout << "Iteration " << Ntry << " completed - Before cleanup" << endl;
     576                //std::cout << "Iteration " << Ntry << " completed - Before cleanup" << std::endl;
    583577                //
    584578                // Cleanup
    … …  
    599593                }
    600594
    601                 //cout << "Iteration " << Ntry << " completed - After cleanup" << endl;
     595                //std::cout << "Iteration " << Ntry << " completed - After cleanup" << std::endl;
    602596        }
    603597        //
    604598        fVtxDone = kTRUE;       // Set fitting completion flag
    605599        //
    606         delete[] fi;            // Phases 
     600        delete[] fi;            // Phases
    607601        delete[] x0i;           // Track expansion point
    608602        delete[] ai;            // dx/dphi
    … …  
    615609TVectorD VertexFit::GetVtx()
    616610{
    617         //cout << "GetVtx: flag set to " << fVtxDone << endl;
     611        //std::cout << "GetVtx: flag set to " << fVtxDone << std::endl;
    618612        if (!fVtxDone)VertexFinder();
    619613        return fXv;
    … …  
    641635void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov)  // Add gaussian vertex constraint
    642636{
    643         cout << "VertexFit::AddVtxConstraint: Not implemented yet" << endl;
     637        std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
    644638}
    645639//
    646640void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov)                   // Add track to input list
    647641{
    648         cout << "VertexFit::AddTrk: Not implemented yet" << endl;
     642        std::cout << "VertexFit::AddTrk: Not implemented yet" << std::endl;
    649643}
    650644void VertexFit::RemoveTrk(Int_t iTrk)                                                   // Remove iTrk track
    651645{
    652         cout << "VertexFit::RemoveTrk: Not implemented yet" << endl;
    653 }
     646        std::cout << "VertexFit::RemoveTrk: Not implemented yet" << std::endl;
     647}
Note: See TracChangeset for help on using the changeset viewer.