Fork me on GitHub

source: git/external/TrackCovariance/SolGridCov.cc@ 5c716fa

Last change on this file since 5c716fa was ff9fb2d9, checked in by Pavel Demin <pavel.demin@…>, 5 years ago

add TrackCovariance

  • Property mode set to 100644
File size: 5.0 KB
Line 
1#include <iostream>
2
3#include <TMath.h>
4#include <TVectorD.h>
5#include <TMatrixDSym.h>
6#include <TDecompChol.h>
7#include <TMatrixDSymEigen.h>
8
9#include "SolGridCov.h"
10#include "SolGeom.h"
11#include "SolTrack.h"
12
13using namespace std;
14
15SolGridCov::SolGridCov()
16{
17 // Define pt-polar angle grid
18 fNpt = 22;
19 fPta.ResizeTo(fNpt);
20 Double_t p[] = { 0.1, 0.2, 0.5, 0.7, 1., 2., 3., 4., 6., 8., 10., 15.,
21 20., 25., 30., 40., 50., 60., 80., 100., 150., 200. };
22 for (Int_t ip = 0; ip < fNpt; ip++) fPta(ip) = p[ip];
23
24 fNang = 13;
25 fAnga.ResizeTo(fNang);
26 Double_t a[] = { 10., 15., 20., 25., 30., 35., 40., 45., 50., 60., 70., 80., 90. };
27 for (Int_t ia = 0; ia < fNang; ia++) fAnga(ia) = a[ia];
28 fCov = new TMatrixDSym[fNpt * fNang];
29 for (Int_t ip = 0; ip < fNpt; ip++)
30 {
31 for (Int_t ia = 0; ia < fNang; ia++) fCov[ip * fNang + ia].ResizeTo(5, 5);
32 }
33}
34
35SolGridCov::~SolGridCov()
36{
37 delete[] fCov;
38}
39
40void SolGridCov::Calc(SolGeom *G)
41{
42 TVectorD pta = fPta;
43 TVectorD anga = fAnga;
44 Bool_t Res = kTRUE; Bool_t MS = kTRUE; // Resolution and multiple scattering flags
45 for (Int_t ip = 0; ip < fNpt; ip++) // Loop on pt grid
46 {
47 Int_t ipt = TMath::Nint(10 * pta(ip));
48 for (Int_t ia = 0; ia < fNang; ia++) // Loop on angle grid
49 {
50 Double_t th = TMath::Pi() * (anga(ia)) / 180.;
51 Double_t x[3], p[3];
52 x[0] = 0; x[1] = 0; x[2] = 0; // Set origin
53 p[0] = pta(ip); p[1] = 0; p[2] = pta(ip) / TMath::Tan(th);
54 //
55 SolTrack *tr = new SolTrack(x, p, G); // Initialize track
56 tr->CovCalc(Res, MS); // Calculate covariance
57 fCov[ip * fNang + ia] = tr->Cov(); // Get covariance
58 }
59 }
60}
61// Find bin in grid
62Int_t SolGridCov::GetMinIndex(Double_t xval, Int_t N, TVectorD x)
63{
64 Int_t min = -1; // default for xval below the lower limit
65 if (xval < x(0))return min;
66 if (xval>x(N - 1)){ min = N; return min; }
67 for (Int_t i = 0; i < N; i++) if (xval>x(i))min = i;
68 return min;
69}
70// Force positive definitness in normalized matrix
71TMatrixDSym SolGridCov::MakePosDef(TMatrixDSym NormMat)
72{
73 // Input: symmetric matrix with 1's on diagonal
74 // Output: positive definite matrix with 1's on diagonal
75
76 // Default return value
77 TMatrixDSym rMatN = NormMat;
78 // Check the diagonal
79 Bool_t Check = kFALSE;
80 Int_t Size = NormMat.GetNcols();
81 for (Int_t i = 0; i < Size; i++)if (TMath::Abs(NormMat(i, i) - 1.0)>1.0E-15)Check = kTRUE;
82 if (Check)
83 {
84 cout << "SolGridCov::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << endl;
85 return rMatN;
86 }
87 // Diagonalize matrix
88 TMatrixDSymEigen Eign(NormMat);
89 TMatrixD U = Eign.GetEigenVectors();
90 TVectorD lambda = Eign.GetEigenValues();
91 // Reset negative eigenvalues to small positive value
92 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13;
93 for (Int_t i = 0; i < Size; i++)
94 {
95 D(i, i) = lambda(i);
96 if (lambda(i) <= 0) D(i, i) = eps;
97 }
98 // Rebuild matrix
99 TMatrixD Ut(TMatrixD::kTransposed, U);
100 TMatrixD rMat = (U*D)*Ut; // Now it is positive defite
101 // Restore all ones on diagonal
102 for (Int_t i1 = 0; i1 < Size; i1++)
103 {
104 Double_t rn1 = TMath::Sqrt(rMat(i1, i1));
105 for (Int_t i2 = 0; i2 <= i1; i2++)
106 {
107 Double_t rn2 = TMath::Sqrt(rMat(i2, i2));
108 rMatN(i1, i2) = 0.5*(rMat(i1, i2) + rMat(i2, i1)) / (rn1*rn2);
109 rMatN(i2, i1) = rMatN(i1, i2);
110 }
111 }
112 return rMatN;
113}
114// Interpolate covariance matrix: Bi-linear interpolation
115TMatrixDSym SolGridCov::GetCov(Double_t pt, Double_t ang)
116{
117 // pt in GeV and ang in degrees
118 Int_t minPt = GetMinIndex(pt, fNpt, fPta);
119 if (minPt == -1)minPt = 0;
120 if (minPt == fNpt - 1)minPt = fNpt - 2;
121 Double_t dpt = fPta(minPt + 1) - fPta(minPt);
122 // Put ang in 0-90 range
123 ang = TMath::Abs(ang);
124 while (ang > 90.)ang -= 90.; // Needs to be fixed
125 Int_t minAng = GetMinIndex(ang, fNang, fAnga);
126 if (minAng == -1)minAng = 0;
127 if (minAng == fNang - 1)minAng = fNang - 2;
128 Double_t dang = fAnga(minAng + 1) - fAnga(minAng);
129 //
130 Double_t tpt = (pt - fPta(minPt)) / dpt;
131 Double_t tang = (ang - fAnga(minAng)) / dang;
132 //
133 TMatrixDSym C11 = fCov[minPt * fNang + minAng];
134 TMatrixDSym C12 = fCov[minPt * fNang + minAng + 1];
135 TMatrixDSym C21 = fCov[(minPt + 1) * fNang + minAng];
136 TMatrixDSym C22 = fCov[(minPt + 1) * fNang + minAng + 1];
137 TMatrixDSym Cv = ((1-tpt) * (1-tang)) * C11 +
138 ((1-tpt) * tang ) * C12 +
139 ( tpt * (1-tang)) * C21 +
140 ( tpt * tang ) * C22;
141 // Check for positive definiteness
142 TMatrixDSym CvN = Cv;
143 TMatrixDSym DCvInv(5); DCvInv.Zero();
144 for (Int_t id = 0; id < 5; id++) DCvInv(id, id) = 1.0 / TMath::Sqrt(Cv(id, id));
145 CvN.Similarity(DCvInv); // Normalize diagonal to 1
146 TDecompChol Chl(CvN);
147 if (!Chl.Decompose())
148 {
149 cout << "SolGridCov::GetCov: Interpolated matrix not positive definite. Recovering ...." << endl;
150 TMatrixDSym rCv = MakePosDef(CvN); CvN = rCv;
151 TMatrixDSym DCv(5); DCv.Zero();
152 for (Int_t id = 0; id < 5; id++) DCv(id, id) = TMath::Sqrt(Cv(id, id));
153 Cv = CvN.Similarity(DCv); // Normalize diagonal to 1
154 }
155
156 return Cv;
157}
Note: See TracBrowser for help on using the repository browser.