Fork me on GitHub

source: git/external/TrackCovariance/VertexFit.cc@ d489660

Last change on this file since d489660 was 82db145, checked in by Franco BEDESCHI <bed@…>, 4 years ago

Vertex fit add/remove tracks - TrkUtil has cluster counting info
  • Property mode set to 100644
File size: 15.2 KB
Line 
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//
11// Empty construction (to be used when adding tracks later with AddTrk() )
12VertexFit::VertexFit()
13{
14 fNtr = 0;
15 fRold = -1.0;
16 fVtxDone = kFALSE;
17 fVtxCst = kFALSE;
18 fxCst.ResizeTo(3);
19 fCovCst.ResizeTo(3, 3);
20 fXv.ResizeTo(3);
21 fcovXv.ResizeTo(3, 3);
22}
23//
24// Build from list of parameters and covariances
25VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
26{
27 fNtr = Ntr;
28 fRold = -1.0;
29 fVtxDone = kFALSE;
30 fVtxCst = kFALSE;
31 fxCst.ResizeTo(3);
32 fCovCst.ResizeTo(3, 3);
33 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
48VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
49{
50 fNtr = Ntr;
51 fRold = -1.0;
52 fVtxDone = kFALSE;
53 fVtxCst = kFALSE;
54 fxCst.ResizeTo(3);
55 fCovCst.ResizeTo(3, 3);
56 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 }
65}
66//
67// Destructor
68//
69void 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}
87VertexFit::~VertexFit()
88{
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();
105 fNtr = 0;
106}
107//
108Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
109{
110 //
111 // Find radius of minimum distance between two tracks
112 //
113 // p = (D,phi, C, z0, ct)
114 //
115 // Define arrays
116 //
117 Double_t C1 = p1(2);
118 Double_t C2 = p2(2);
119 Double_t ph1 = p1(1);
120 Double_t ph2 = p2(1);
121 TVectorD x0 = Fill_x0(p1);
122 TVectorD y0 = Fill_x0(p2);
123 TVectorD n = Fill_a(p1, 0.0);
124 n *= (2*C1);
125 TVectorD k = Fill_a(p2, 0.0);
126 k *= (2*C2);
127 //
128 // Setup and solve linear system
129 //
130 Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
131 Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
132 Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
133 Double_t discr = nn * kk - nk * nk;
134 TMatrixDSym H(2);
135 H(0, 0) = kk;
136 H(0, 1) = nk;
137 H(1, 0) = H(0, 1);
138 H(1, 1) = nn;
139 TVectorD c(2);
140 c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
141 c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
142 TVectorD smin = (H * c);
143 smin *= 1.0 / discr;
144 //
145 TVectorD X = x0 + smin(0) * n;
146 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
161Double_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 }
192 }
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
203 //
204 return R;
205}
206//
207// Regularized symmetric matrix inversion
208//
209TMatrixDSym 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
238 }
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;
247 }
248 }
249 //****************
250 // case N > 2 ***
251 //****************
252 else
253 {
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;
305}
306//
307//
308//
309TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
310{
311 //
312 // Derivative of track 3D position vector with respect to track parameters at constant phase
313 //
314 // par = vector of track parameters
315 // phi = phase
316 //
317 TMatrixD A(3, 5);
318 //
319 // Decode input arrays
320 //
321 Double_t D = par(0);
322 Double_t p0 = par(1);
323 Double_t C = par(2);
324 Double_t z0 = par(3);
325 Double_t ct = par(4);
326 //
327 // Fill derivative matrix dx/d alpha
328 // D
329 A(0, 0) = -TMath::Sin(p0);
330 A(1, 0) = TMath::Cos(p0);
331 A(2, 0) = 0.0;
332 // 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);
335 A(2, 1) = 0.0;
336 // 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);
340 // z0
341 A(0, 3) = 0.0;
342 A(1, 3) = 0.0;
343 A(2, 3) = 1.0;
344 // ct = lambda
345 A(0, 4) = 0.0;
346 A(1, 4) = 0.0;
347 A(2, 4) = phi / (2 * C);
348 //
349 return A;
350}
351//
352TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
353{
354 //
355 // Derivative of track 3D position vector with respect to phase at constant track parameters
356 //
357 // par = vector of track parameters
358 // phi = phase
359 //
360 TVectorD a(3);
361 //
362 // Decode input arrays
363 //
364 Double_t D = par(0);
365 Double_t p0 = par(1);
366 Double_t C = par(2);
367 Double_t z0 = par(3);
368 Double_t ct = par(4);
369 //
370 a(0) = TMath::Cos(phi + p0) / (2 * C);
371 a(1) = TMath::Sin(phi + p0) / (2 * C);
372 a(2) = ct / (2 * C);
373 //
374 return a;
375}
376//
377TVectorD VertexFit::Fill_x0(TVectorD par)
378{
379 //
380 // Calculate track 3D position at R = |D| (minimum approach to z-axis)
381 //
382 TVectorD x0(3);
383 //
384 // Decode input arrays
385 //
386 Double_t D = par(0);
387 Double_t p0 = par(1);
388 Double_t C = par(2);
389 Double_t z0 = par(3);
390 Double_t ct = par(4);
391 //
392 x0(0) = -D * TMath::Sin(p0);
393 x0(1) = D * TMath::Cos(p0);
394 x0(2) = z0;
395 //
396 return x0;
397}
398//
399TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
400{
401 //
402 // Calculate track 3D position for a given phase, phi
403 //
404 TVectorD x(3);
405 //
406 // Decode input arrays
407 //
408 Double_t D = par(0);
409 Double_t p0 = par(1);
410 Double_t C = par(2);
411 Double_t z0 = par(3);
412 Double_t ct = par(4);
413 //
414 TVectorD x0 = Fill_x0(par);
415 x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
416 x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
417 x(2) = x0(2) + ct * phi / (2 * C);
418 //
419 return x;
420}
421//
422void 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//
455void 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
465 //
466 // Initial variable definitions
467 TVectorD x(3);
468 TMatrixDSym covX(3);
469 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
477 //
478 // Iteration properties
479 //
480 Int_t Ntry = 0;
481 Int_t TryMax = 100;
482 Double_t eps = 1.0e-9; // vertex stability
483 Double_t epsi = 1000.;
484 //
485 // Iteration loop
486 while (epsi > eps && Ntry < TryMax) // Iterate until found vertex is stable
487 {
488 // Initialize arrays
489 x.Zero();
490 TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
491 covX.Zero(); // Reset vertex covariance
492 cterm.Zero(); // Reset constant term
493 H.Zero(); // Reset H matrix
494 DW1D.Zero();
495 //
496 // Reset work arrays
497 //
498 ResetWrkArrays();
499 //
500 // Start loop on tracks
501 //
502 for (Int_t i = 0; i < fNtr; i++)
503 {
504 // Get track helix parameters and their covariance matrix
505 TVectorD par = *fPar[i];
506 TMatrixDSym Cov = *fCov[i];
507 //
508 // For first iteration only
509 Double_t fs;
510 if (Ntry <= 0) // Initialize all phases on first pass
511 {
512 Double_t D = par(0);
513 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);
517 }
518 //
519 // Update track related arrays
520 //
521 UpdateTrkArrays(i);
522 TMatrixDSym Ds = *fDi[i];
523 TMatrixDSym Winv = *fWinvi[i];
524 TMatrixDSym DsW1Ds = Winv.Similarity(Ds); // Service matrix to calculate covX
525 //
526 // Update global arrays
527 DW1D += DsW1Ds;
528 // Update hessian
529 H += Ds;
530 // update constant term
531 TVectorD xs = *fx0i[i];
532 cterm += Ds * xs;
533 } // End loop on tracks
534 //
535 // update vertex position
536 TMatrixDSym H1 = RegInv(H);
537 x = H1 * cterm;
538 //
539 // Update vertex covariance
540 covX = DW1D.Similarity(H1);
541 //
542 // Update phases and chi^2
543 Chi2 = 0.0;
544 for (Int_t i = 0; i < fNtr; i++)
545 {
546 TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
547 TMatrixDSym Wm1 = *fWinvi[i];
548 fChi2List(i) = Wm1.Similarity(lambda);
549 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];
553 }
554 //
555 TVectorD dx = x - x0;
556 x0 = x;
557 // update vertex stability
558 TMatrixDSym Hess = RegInv(covX);
559 epsi = Hess.Similarity(dx);
560 Ntry++;
561 //
562 // Store result
563 //
564 fXv = x; // Vertex position
565 fcovXv = covX; // Vertex covariance
566 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
575TVectorD VertexFit::GetVtx()
576{
577 if (!fVtxDone) VertexFitter();
578 return fXv;
579}
580//
581// Return fit vertex covariance
582TMatrixDSym VertexFit::GetVtxCov()
583{
584 if (!fVtxDone) VertexFitter();
585 return fcovXv;
586}
587//
588// Return fit vertex chi2
589Double_t VertexFit::GetVtxChi2()
590{
591 if (!fVtxDone) VertexFitter();
592 return fChi2;
593}
594//
595// Return array of chi2 contributions from each track
596TVectorD VertexFit::GetVtxChi2List()
597{
598 if (!fVtxDone) VertexFitter();
599 return fChi2List;
600}
601//
602// Handle tracks/constraints
603void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov) // Add gaussian vertex constraint
604{
605 std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
606}
607//
608// Adding tracks one by one
609void 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
623void 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}
Note: See TracBrowser for help on using the repository browser.