1 | #include <TMath.h>
|
---|
2 | #include <TVectorD.h>
|
---|
3 | #include <TVector3.h>
|
---|
4 | #include <TMatrixD.h>
|
---|
5 | #include <TMatrixDSym.h>
|
---|
6 | #include "VertexFit.h"
|
---|
7 | //
|
---|
8 | // Constructors
|
---|
9 | //
|
---|
10 | // Empty
|
---|
11 | VertexFit::VertexFit()
|
---|
12 | {
|
---|
13 | fNtr = 0;
|
---|
14 | fVtxDone = kFALSE;
|
---|
15 | fVtxCst = kFALSE;
|
---|
16 | fxCst.ResizeTo(3);
|
---|
17 | fCovCst.ResizeTo(3,3);
|
---|
18 | fXv.ResizeTo(3);
|
---|
19 | fcovXv.ResizeTo(3,3);
|
---|
20 | }
|
---|
21 | // Parameters and covariances
|
---|
22 | VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
|
---|
23 | {
|
---|
24 | fNtr = Ntr;
|
---|
25 | fVtxDone = kFALSE;
|
---|
26 | fVtxCst = kFALSE;
|
---|
27 | fxCst.ResizeTo(3);
|
---|
28 | fCovCst.ResizeTo(3,3);
|
---|
29 | 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
|
---|
51 | VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
|
---|
52 | {
|
---|
53 | fNtr = Ntr;
|
---|
54 | fVtxDone = kFALSE;
|
---|
55 | fVtxCst = kFALSE;
|
---|
56 | fxCst.ResizeTo(3);
|
---|
57 | fCovCst.ResizeTo(3,3);
|
---|
58 | 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 | //cout << "fPar = " << endl; fPar[i]->Print();
|
---|
68 | fCov[i] = new TMatrixDSym(track[i]->GetCov());
|
---|
69 | //cout << "fCov = " << 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 | //cout << "VertexFit constructor executed" << endl;
|
---|
86 | }
|
---|
87 | //
|
---|
88 | // Destructor
|
---|
89 | VertexFit::~VertexFit()
|
---|
90 | {
|
---|
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 | }
|
---|
108 | 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
|
---|
124 | //
|
---|
125 | // p = (D,phi, C, z0, ct)
|
---|
126 | //
|
---|
127 | // Define arrays
|
---|
128 | //
|
---|
129 | Double_t r1 = 1.0 / p1(2);
|
---|
130 | Double_t r2 = 1.0 / p2(2);
|
---|
131 | TVectorD x0 = Fill_x0(p1);
|
---|
132 | TVectorD y0 = Fill_x0(p2);
|
---|
133 | TVectorD n = Fill_a(p1, 0.0);
|
---|
134 | n *= r1;
|
---|
135 | TVectorD k = Fill_a(p2, 0.0);
|
---|
136 | k *= r2;
|
---|
137 | //
|
---|
138 | // Setup and solve linear system
|
---|
139 | //
|
---|
140 | Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
|
---|
141 | Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
|
---|
142 | Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
|
---|
143 | Double_t discr = nn * kk - nk * nk;
|
---|
144 | TMatrixDSym H(2);
|
---|
145 | H(0, 0) = kk;
|
---|
146 | H(0, 1) = nk;
|
---|
147 | H(1, 0) = H(0, 1);
|
---|
148 | H(1, 1) = nn;
|
---|
149 | TVectorD c(2);
|
---|
150 | c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
|
---|
151 | c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
|
---|
152 | TVectorD smin = (H * c);
|
---|
153 | smin *= 1.0 / discr;
|
---|
154 | //
|
---|
155 | TVectorD X = x0 + smin(0) * n;
|
---|
156 | 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)
|
---|
196 | {
|
---|
197 | cout << "FastRv: maximum number of iteration reached" << endl;
|
---|
198 | break;
|
---|
199 | }
|
---|
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 | }
|
---|
206 | //
|
---|
207 | return R;
|
---|
208 | }
|
---|
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 | cout << "RegInv3 called with matrix size != 3. Abort & return standard inversion." << 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);
|
---|
233 | }
|
---|
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);
|
---|
255 | }
|
---|
256 | InvReg(2, 2) = -Det;
|
---|
257 | //
|
---|
258 | InvReg *= 1.0 / h;
|
---|
259 | //
|
---|
260 | //
|
---|
261 | return InvReg.Similarity(D);
|
---|
262 | }
|
---|
263 | else
|
---|
264 | {
|
---|
265 | cout << "RegInv3: found negative elements in diagonal. Return standard inversion." << endl;
|
---|
266 | return Smat.Invert();
|
---|
267 | }
|
---|
268 | }
|
---|
269 | //
|
---|
270 | //
|
---|
271 | //
|
---|
272 | TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
|
---|
273 | {
|
---|
274 | //
|
---|
275 | // Derivative of track 3D position vector with respect to track parameters at constant phase
|
---|
276 | //
|
---|
277 | // par = vector of track parameters
|
---|
278 | // phi = phase
|
---|
279 | //
|
---|
280 | TMatrixD A(3, 5);
|
---|
281 | //
|
---|
282 | // Decode input arrays
|
---|
283 | //
|
---|
284 | Double_t D = par(0);
|
---|
285 | Double_t p0 = par(1);
|
---|
286 | Double_t C = par(2);
|
---|
287 | Double_t z0 = par(3);
|
---|
288 | Double_t ct = par(4);
|
---|
289 | //
|
---|
290 | // Fill derivative matrix dx/d alpha
|
---|
291 | // D
|
---|
292 | A(0, 0) = -TMath::Sin(p0);
|
---|
293 | A(1, 0) = TMath::Cos(p0);
|
---|
294 | A(2, 0) = 0.0;
|
---|
295 | // 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);
|
---|
298 | A(2, 1) = 0.0;
|
---|
299 | // 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);
|
---|
303 | // z0
|
---|
304 | A(0, 3) = 0.0;
|
---|
305 | A(1, 3) = 0.0;
|
---|
306 | A(2, 3) = 1.0;
|
---|
307 | // ct = lambda
|
---|
308 | A(0, 4) = 0.0;
|
---|
309 | A(1, 4) = 0.0;
|
---|
310 | A(2, 4) = phi / (2 * C);
|
---|
311 | //
|
---|
312 | return A;
|
---|
313 | }
|
---|
314 | //
|
---|
315 | TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
|
---|
316 | {
|
---|
317 | //
|
---|
318 | // Derivative of track 3D position vector with respect to phase at constant track parameters
|
---|
319 | //
|
---|
320 | // par = vector of track parameters
|
---|
321 | // phi = phase
|
---|
322 | //
|
---|
323 | TVectorD a(3);
|
---|
324 | //
|
---|
325 | // Decode input arrays
|
---|
326 | //
|
---|
327 | Double_t D = par(0);
|
---|
328 | Double_t p0 = par(1);
|
---|
329 | Double_t C = par(2);
|
---|
330 | Double_t z0 = par(3);
|
---|
331 | Double_t ct = par(4);
|
---|
332 | //
|
---|
333 | a(0) = TMath::Cos(phi + p0) / (2 * C);
|
---|
334 | a(1) = TMath::Sin(phi + p0) / (2 * C);
|
---|
335 | a(2) = ct / (2 * C);
|
---|
336 | //
|
---|
337 | return a;
|
---|
338 | }
|
---|
339 | //
|
---|
340 | TVectorD VertexFit::Fill_x0(TVectorD par)
|
---|
341 | {
|
---|
342 | //
|
---|
343 | // Calculate track 3D position at R = |D| (minimum approach to z-axis)
|
---|
344 | //
|
---|
345 | TVectorD x0(3);
|
---|
346 | //
|
---|
347 | // Decode input arrays
|
---|
348 | //
|
---|
349 | Double_t D = par(0);
|
---|
350 | Double_t p0 = par(1);
|
---|
351 | Double_t C = par(2);
|
---|
352 | Double_t z0 = par(3);
|
---|
353 | Double_t ct = par(4);
|
---|
354 | //
|
---|
355 | x0(0) = -D *TMath::Sin(p0);
|
---|
356 | x0(1) = D*TMath::Cos(p0);
|
---|
357 | x0(2) = z0;
|
---|
358 | //
|
---|
359 | return x0;
|
---|
360 | }
|
---|
361 | //
|
---|
362 | TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
|
---|
363 | {
|
---|
364 | //
|
---|
365 | // Calculate track 3D position for a given phase, phi
|
---|
366 | //
|
---|
367 | TVectorD x(3);
|
---|
368 | //
|
---|
369 | // Decode input arrays
|
---|
370 | //
|
---|
371 | Double_t D = par(0);
|
---|
372 | Double_t p0 = par(1);
|
---|
373 | Double_t C = par(2);
|
---|
374 | Double_t z0 = par(3);
|
---|
375 | Double_t ct = par(4);
|
---|
376 | //
|
---|
377 | TVectorD x0 = Fill_x0(par);
|
---|
378 | x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
|
---|
379 | x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
|
---|
380 | x(2) = x0(2) + ct*phi / (2 * C);
|
---|
381 | //
|
---|
382 | return x;
|
---|
383 | }
|
---|
384 | //
|
---|
385 | void VertexFit::VertexFinder()
|
---|
386 | {
|
---|
387 | //
|
---|
388 | // Vertex fit (units are meters)
|
---|
389 | //
|
---|
390 | // Initial variable definitions
|
---|
391 | TVectorD x(3);
|
---|
392 | TMatrixDSym covX(3);
|
---|
393 | TVectorD x0(3); for (Int_t v = 0; v < 3; v++)x0(v) = 100.; // set to large value
|
---|
394 | 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 | //cout << "VertexFinder: fast R = " << R << endl;
|
---|
450 | //
|
---|
451 | // Iteration properties
|
---|
452 | //
|
---|
453 | Int_t Ntry = 0;
|
---|
454 | Int_t TryMax = 100;
|
---|
455 | Double_t eps = 1.0e-9; // vertex stability
|
---|
456 | Double_t epsi = 1000.;
|
---|
457 | //
|
---|
458 | while (epsi > eps && Ntry < TryMax) // Iterate until found vertex is stable
|
---|
459 | {
|
---|
460 | x.Zero();
|
---|
461 | TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
|
---|
462 | covX.Zero(); // Reset vertex covariance
|
---|
463 | cterm.Zero(); // Reset constant term
|
---|
464 | H.Zero(); // Reset H matrix
|
---|
465 | DW1D.Zero();
|
---|
466 | //
|
---|
467 | //cout << "VertexFinder: start loop on tracks" << endl;
|
---|
468 | for (Int_t i = 0; i < fNtr; i++)
|
---|
469 | {
|
---|
470 | // Get track helix parameters and their covariance matrix
|
---|
471 | //ObsTrk *t = tracks[i];
|
---|
472 | TVectorD par = *fPar[i];
|
---|
473 | TMatrixDSym Cov = *fCov[i];
|
---|
474 | Double_t fs;
|
---|
475 | if (Ntry <= 0) // Initialize all phases on first pass
|
---|
476 | {
|
---|
477 | Double_t D = par(0);
|
---|
478 | 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;
|
---|
482 | }
|
---|
483 | //
|
---|
484 | // Starting values
|
---|
485 | //
|
---|
486 | fs = fi[i]; // Get phase
|
---|
487 | //cout << "VertexFinder: phase fs set" << endl;
|
---|
488 | TVectorD xs = Fill_x(par, fs);
|
---|
489 | //cout << "VertexFinder: position xs set" << endl;
|
---|
490 | x0i[i] = new TVectorD(xs); // Start helix position
|
---|
491 | //cout << "VertexFinder: position x0i stored" << 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 | //cout << "VertexFinder: derivatives A set" << endl;
|
---|
495 | TMatrixDSym Winv = Cov.Similarity(A); // W^-1 = A*C*A'
|
---|
496 | Winvi[i] = new TMatrixDSym(Winv); // Store W^-1 matrix
|
---|
497 | //cout << "VertexFinder: Winvi stored" << endl;
|
---|
498 | TMatrixDSym W = RegInv3(Winv); // W = (A*C*A')^-1
|
---|
499 | Wi[i] = new TMatrixDSym(W); // Store W matrix
|
---|
500 | //cout << "VertexFinder: Wi stored" << endl;
|
---|
501 | TVectorD a = Fill_a(par, fs); // a = dx/ds = derivatives wrt phase
|
---|
502 | //cout << "VertexFinder: derivatives a set" << endl;
|
---|
503 | ai[i] = new TVectorD(a); // Store a
|
---|
504 | //cout << "VertexFinder: derivatives a stored" << 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 | //cout << "VertexFinder: matrix Di stored" << endl;
|
---|
515 | TMatrixDSym DsW1Ds = Winv.Similarity(Ds); // Service matrix to calculate covX
|
---|
516 | DW1D += DsW1Ds;
|
---|
517 | // Update hessian
|
---|
518 | H += Ds;
|
---|
519 | // update constant term
|
---|
520 | cterm += Ds * xs;
|
---|
521 | } // End loop on tracks
|
---|
522 | //
|
---|
523 | // update vertex position
|
---|
524 | TMatrixDSym H1 = RegInv3(H);
|
---|
525 | x = H1*cterm;
|
---|
526 | //cout << "VertexFinder: x vertex set" << endl;
|
---|
527 | // Update vertex covariance
|
---|
528 | covX = DW1D.Similarity(H1);
|
---|
529 | //cout << "VertexFinder: cov vertex set" << endl;
|
---|
530 | // Update phases and chi^2
|
---|
531 | Chi2 = 0.0;
|
---|
532 | for (Int_t i = 0; i < fNtr; i++)
|
---|
533 | {
|
---|
534 | TVectorD lambda = (*Di[i])*(*x0i[i] - x);
|
---|
535 | TMatrixDSym Wm1 = *Winvi[i];
|
---|
536 | fChi2List(i) = Wm1.Similarity(lambda);
|
---|
537 | 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];
|
---|
541 | }
|
---|
542 |
|
---|
543 | //cout << "VertexFinder: end chi2 calculation" << endl;
|
---|
544 | //
|
---|
545 | TVectorD dx = x - x0;
|
---|
546 | x0 = x;
|
---|
547 | // update vertex stability
|
---|
548 | TMatrixDSym Hess = RegInv3(covX);
|
---|
549 | epsi = Hess.Similarity(dx);
|
---|
550 | Ntry++;
|
---|
551 | //
|
---|
552 | // Store result
|
---|
553 | //
|
---|
554 | fXv = x; // Vertex position
|
---|
555 | fcovXv = covX; // Vertex covariance
|
---|
556 | fChi2 = Chi2; // Vertex fit Chi2
|
---|
557 | //
|
---|
558 | // Store intermediate data
|
---|
559 | //
|
---|
560 |
|
---|
561 | //cout << "VertexFinder: before store intermediate data" << endl;
|
---|
562 | for (Int_t i = 0; i < fNtr; i++)
|
---|
563 | {
|
---|
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;
|
---|
567 | ffi[i] = fi[i]; // Fit phases
|
---|
568 | //cout << "VertexFinder: fi stored" << endl;
|
---|
569 | fx0i[i] = x0i[i]; // Track expansion points
|
---|
570 | //cout << "VertexFinder: x0i stored" << endl;
|
---|
571 | fai[i] = ai[i]; // dx/dphi
|
---|
572 | //cout << "VertexFinder: ai stored" << endl;
|
---|
573 | fa2i[i] = a2i[i]; // a'Wa
|
---|
574 | //cout << "VertexFinder: a2i stored" << endl;
|
---|
575 | fDi[i] = Di[i]; // W-WBW
|
---|
576 | //cout << "VertexFinder: Di stored" << endl;
|
---|
577 | fWi[i] = Wi[i]; // (ACA')^-1
|
---|
578 | //cout << "VertexFinder: Wi stored" << endl;
|
---|
579 | fWinvi[i] = Winvi[i]; // ACA'
|
---|
580 | //cout << "VertexFinder: Winvi stored" << endl;
|
---|
581 | }
|
---|
582 | //cout << "Iteration " << Ntry << " completed - Before cleanup" << 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 | //cout << "Iteration " << Ntry << " completed - After cleanup" << 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 | //
|
---|
615 | TVectorD VertexFit::GetVtx()
|
---|
616 | {
|
---|
617 | //cout << "GetVtx: flag set to " << fVtxDone << endl;
|
---|
618 | if (!fVtxDone)VertexFinder();
|
---|
619 | return fXv;
|
---|
620 | }
|
---|
621 |
|
---|
622 | TMatrixDSym VertexFit::GetVtxCov()
|
---|
623 | {
|
---|
624 | if (!fVtxDone)VertexFinder();
|
---|
625 | return fcovXv;
|
---|
626 | }
|
---|
627 |
|
---|
628 | Double_t VertexFit::GetVtxChi2()
|
---|
629 | {
|
---|
630 | if (!fVtxDone)VertexFinder();
|
---|
631 | return fChi2;
|
---|
632 | }
|
---|
633 |
|
---|
634 | TVectorD VertexFit::GetVtxChi2List()
|
---|
635 | {
|
---|
636 | if (!fVtxDone)VertexFinder();
|
---|
637 | return fChi2List;
|
---|
638 | }
|
---|
639 | //
|
---|
640 | // Handle tracks/constraints
|
---|
641 | void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov) // Add gaussian vertex constraint
|
---|
642 | {
|
---|
643 | cout << "VertexFit::AddVtxConstraint: Not implemented yet" << endl;
|
---|
644 | }
|
---|
645 | //
|
---|
646 | void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov) // Add track to input list
|
---|
647 | {
|
---|
648 | cout << "VertexFit::AddTrk: Not implemented yet" << endl;
|
---|
649 | }
|
---|
650 | void VertexFit::RemoveTrk(Int_t iTrk) // Remove iTrk track
|
---|
651 | {
|
---|
652 | cout << "VertexFit::RemoveTrk: Not implemented yet" << endl;
|
---|
653 | }
|
---|