Fork me on GitHub

Ignore:
File:
1 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/VertexFit.cc

    r82db145 r53f4746  
    1818        fxCst.ResizeTo(3);
    1919        fCovCst.ResizeTo(3, 3);
     20        fCovCstInv.ResizeTo(3, 3);
    2021        fXv.ResizeTo(3);
    2122        fcovXv.ResizeTo(3, 3);
    … …  
    3132        fxCst.ResizeTo(3);
    3233        fCovCst.ResizeTo(3, 3);
     34        fCovCstInv.ResizeTo(3, 3);
    3335        fXv.ResizeTo(3);
    3436        fcovXv.ResizeTo(3, 3);
    … …  
    3840                TVectorD pr = *trkPar[i];
    3941                fPar.push_back(new TVectorD(pr));
     42                fParNew.push_back(new TVectorD(pr));
    4043                TMatrixDSym cv = *trkCov[i];
    4144                fCov.push_back(new TMatrixDSym(cv));
     45                fCovNew.push_back(new TMatrixDSym(cv));
    4246        }
    4347        fChi2List.ResizeTo(fNtr);
    … …  
    5458        fxCst.ResizeTo(3);
    5559        fCovCst.ResizeTo(3, 3);
     60        fCovCstInv.ResizeTo(3, 3);
    5661        fXv.ResizeTo(3);
    5762        fcovXv.ResizeTo(3, 3);
    … …  
    6166        {
    6267                fPar.push_back(new TVectorD(track[i]->GetObsPar()));
     68                fParNew.push_back(new TVectorD(track[i]->GetObsPar()));
    6369                fCov.push_back(new TMatrixDSym(track[i]->GetCov()));
     70                fCovNew.push_back(new TMatrixDSym(track[i]->GetCov()));
    6471        }
    6572}
    … …  
    7380        {
    7481                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]; }
     82                if (fai[i])   { fai[i]->Clear();                delete fai[i]; }
     83                if (fdi[i])   { fdi[i]->Clear();                delete fdi[i]; }
     84                if (fAti[i])  { fAti[i]->Clear();               delete fAti[i]; }
     85                if (fDi[i])   { fDi[i]->Clear();                delete fDi[i]; }
     86                if (fWi[i])   { fWi[i]->Clear();                delete fWi[i]; }
    7887                if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
    7988        }
    … …  
    8190        fx0i.clear();
    8291        fai.clear();
     92        fdi.clear();
     93        fAti.clear();
    8394        fDi.clear();
    8495        fWi.clear();
    … …  
    8899{
    89100        fxCst.Clear();         
    90         fCovCst.Clear();               
     101        fCovCst.Clear();
     102        fCovCstInv.Clear();
    91103        fXv.Clear();           
    92104        fcovXv.Clear();         
    … …  
    95107        for (Int_t i = 0; i < fNtr; i++)
    96108        {
    97                 fPar[i]->Clear();       delete fPar[i];
    98                 fCov[i]->Clear();       delete fCov[i];
     109                fPar[i]->Clear();               delete fPar[i];
     110                fParNew[i]->Clear();    delete fParNew[i];
     111                fCov[i]->Clear();               delete fCov[i];
     112                fCovNew[i]->Clear();    delete fCovNew[i];
    99113        }
    100114        fPar.clear();
    101         fCov.clear();   
     115        fParNew.clear();
     116        fCov.clear();
     117        fCovNew.clear();               
    102118        //
    103119        ResetWrkArrays();
    … …  
    424440        //
    425441        // Get track parameters and their covariance
    426         TVectorD par = *fPar[i];
     442        TVectorD par = *fParNew[i];
    427443        TMatrixDSym Cov = *fCov[i];
    428444        //
    … …  
    433449        //
    434450        TMatrixD A = Fill_A(par, fs);                           // A = dx/da = derivatives wrt track parameters
     451        TMatrixD At(TMatrixD::kTransposed, A);          // A transposed
     452        fAti.push_back(new TMatrixD(At));                       // Store A'
     453        TVectorD di = A * (par - *fPar[i]);             // x-shift
     454        fdi.push_back(new TVectorD(di));                        // Store x-shift
    435455        TMatrixDSym Winv = Cov.Similarity(A);           // W^-1 = A*C*A'
    436         fWinvi.push_back(new TMatrixDSym(Winv));                // Store W^-1 matrix
     456        fWinvi.push_back(new TMatrixDSym(Winv));        // Store W^-1 matrix
    437457        //
    438458        TMatrixDSym W = RegInv(Winv);                           // W = (A*C*A')^-1
    … …  
    456476{
    457477        //std::cout << "VertexFitter: just in" << std::endl;
    458         if (fNtr < 2)
    459         {
     478        if (fNtr < 2 && !fVtxCst){
    460479                std::cout << "VertexFit::VertexFitter - Method called with less than 2 tracks - Aborting " << std::endl;
    461480                std::exit(1);
    … …  
    469488        Double_t Chi2 = 0;
    470489        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
     490        if (fRold < 0.0) {
     491                // External constraint
     492                if (fVtxCst) fRold = TMath::Sqrt(fxCst(0) * fxCst(0) + fxCst(1) * fxCst(1));
     493                // No constraint
     494                else for (Int_t i = 0; i < 3; i++)x0(i) = 1000.;        // Set to arbitrary large value
     495        }
    472496        //
    473497        // Starting vertex radius approximation
    … …  
    475499        Double_t R = fRold;                                             // Use previous fit if available
    476500        if (R < 0.0) R = StartRadius();                 // Rough vertex estimate
     501        //std::cout << "Start radius: " << R << std::endl;
    477502        //
    478503        // Iteration properties
    … …  
    529554                        H += Ds;
    530555                        // update constant term
    531                         TVectorD xs = *fx0i[i];
     556                        TVectorD xs = *fx0i[i] - *fdi[i];
     557                        TVectorD xx0 = *fx0i[i];
     558                        /*
     559                        std::cout << "Iter. " << Ntry << ", trk " << i << ", x= "
     560                                << xx0(0) << ", " << xx0(1) << ", " << xx0(2)<<
     561                                ", ph0= "<<par(1)<< std::endl;
     562                        */
    532563                        cterm += Ds * xs;
    533564                }                               // End loop on tracks
     565                // Some additions in case of external constraints
     566                if (fVtxCst) {
     567                        H += fCovCstInv;
     568                        cterm += fCovCstInv * fxCst;
     569                        DW1D += fCovCstInv;
     570                }
    534571                //
    535572                // update vertex position
    … …  
    544581                for (Int_t i = 0; i < fNtr; i++)
    545582                {
    546                         TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
     583                        TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x - *fdi[i]);
    547584                        TMatrixDSym Wm1 = *fWinvi[i];
    548585                        fChi2List(i) = Wm1.Similarity(lambda);
    … …  
    551588                        TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));
    552589                        for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
     590                        TVectorD newPar = *fPar[i] - ((*fCov[i]) * (*fAti[i])) * lambda;
     591                        fParNew[i] = new TVectorD(newPar);
    553592                }
     593                // Add external constraint to Chi2
     594                if (fVtxCst) Chi2 += fCovCstInv.Similarity(x - fxCst);
    554595                //
    555596                TVectorD dx = x - x0;
    … …  
    562603                // Store result
    563604                //
    564                 fXv = x;                                // Vertex position
     605                fXv = x;                        // Vertex position
    565606                fcovXv = covX;          // Vertex covariance
    566607                fChi2 = Chi2;           // Vertex fit Chi2
     608                //std::cout << "Found vertex " << fXv(0) << ", " << fXv(1) << ", " << fXv(2) << std::endl;
    567609        }               // end of iteration loop
    568610        //
    … …  
    603645void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov)  // Add gaussian vertex constraint
    604646{
    605         std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
     647        //std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
     648        fVtxCst = kTRUE;                                // Vertex constraint flag
     649        fxCst = xv;                                             // Constraint value
     650        fCovCst = cov;                                  // Constraint covariance
     651        fCovCstInv = cov;
     652        fCovCstInv.Invert();                            // Its inverse
     653        //
     654        // Set starting vertex as external constraint
     655        fXv = fxCst;
     656        fcovXv = fCovCst;
    606657}
    607658//
    … …  
    613664        fPar.push_back(par);                    // add new track
    614665        fCov.push_back(Cov);
     666        fParNew.push_back(par);                 // add new track
     667        fCovNew.push_back(Cov);
    615668        //
    616669        // Reset previous vertex temp arrays
    … …  
    628681        fPar.erase(fPar.begin() + iTrk);                // Remove track
    629682        fCov.erase(fCov.begin() + iTrk);
     683        fParNew.erase(fParNew.begin() + iTrk);          // Remove track
     684        fCovNew.erase(fCovNew.begin() + iTrk);
    630685        //
    631686        // Reset previous vertex temp arrays
Note: See TracChangeset for help on using the changeset viewer.