BmnRoot
Loading...
Searching...
No Matches
BmnKFPrimaryVertexFinder.cxx
Go to the documentation of this file.
1
8#include "BmnKFPrimaryVertexFinder.h"
9
10#include "CbmKF.h"
11#include "CbmKFMath.h"
12#include "CbmKFTrack.h"
13#include "CbmKFVertex.h"
14#include "CbmStsTrack.h"
15#include "CbmVertex.h"
16#include "FairRootManager.h"
17
18#include <TCanvas.h>
19#include <TF1.h>
20#include <TFitResult.h> // AZ-310525
21#include <TH2.h> // AZ-300525
22#include <TRandom.h> // AZ-200226
23#include <vector>
24using std::vector;
25
26//__________________________________________________________________________
27
28InitStatus BmnKFPrimaryVertexFinder::Init()
29{
30
31 fInputTracks = (TClonesArray*)FairRootManager::Instance()->GetObject("StsVector");
32 if (fInputTracks == 0x0)
33 fInputTracks = (TClonesArray*)FairRootManager::Instance()->GetObject("StsTrack");
34
35 // Create and register CbmVertex object
36 fPrimVert = new CbmVertex("PrimaryVertex", "Global");
37 FairRootManager::Instance()->Register("MpdVertex.", "Global", fPrimVert, kTRUE);
38 fHist[0] = new TH1D("hX", "Vertex X", 100, -5, 5);
39 fHist[1] = new TH1D("hY", "Vertex Y", 100, -5, 5);
40 fHistw[0] = new TH1D("hXw", "Vertex X", 100, -5, 5);
41 fHistw[1] = new TH1D("hYw", "Vertex Y", 100, -5, 5);
42 fHistPca = new TH1D("hPca", "PCA", 200, -10, 10);
43 fHistErr[0] = new TH1D("hErrX", "X-error", 100, 0, 1.0);
44 fHistErr[1] = new TH1D("hErrY", "Y-error", 100, 0, 1.0);
45 fHistR = new TH1D("hRad", "Radius", 50, 0, 10);
46
47 // AZ-300525
48 Double_t zmin = -141, zmax = 39;
49 int nz = 30;
50 fHist3 = new TH3D("h3", "XYZ", 50, -10, 10, 50, -10, 10, nz, zmin, zmax);
51 fHistw3 = new TH3D("hw3", "XYZ", 50, -10, 10, 50, -10, 10, nz, zmin, zmax);
52 fHistZ = new TH1D("hZ", "Z-profile", nz, zmin, zmax);
53 fHistZ->SetStats(kFALSE);
54 fHZR = new TH2D("hZR", "Z-profile", nz, zmin, zmax, 50, 0.0, 10.0);
55 return kSUCCESS;
56}
57
58//__________________________________________________________________________
59
60void BmnKFPrimaryVertexFinder::Exec(Option_t* option)
61{
62
63 // static int eventCounter = 0;
64 // cout << " VertexFinder event " << ++eventCounter << endl;
65
66 Clear();
67
68 if (fInputTracks == nullptr)
69 return;
70
71 const Double_t c2cut = 3.5 * 3.5;
72 Int_t nTracks = fInputTracks->GetEntriesFast();
73
74 CbmKFTrack* cloneArray = new CbmKFTrack[nTracks];
75
76 // <AZ-270425 - sort tracks according to quality
77 std::multimap<float, int> mQuInd;
78 for (int i = 0; i < nTracks; ++i) {
79 CbmStsTrack* st = (CbmStsTrack*)fInputTracks->UncheckedAt(i);
80 // if (st->GetB() < -0.1) continue; // AZ-300525 - TMVA cut
81 Int_t nHits = st->GetNStsHits();
82 float qual = nHits;
83 qual += (10 - TMath::Min(10.0, st->GetChi2() / st->GetNDF())) / 11;
84 mQuInd.insert(pair<float, int>(qual, i));
85 }
86 int iii = 0;
87 // >AZ
88
89 // AZ-270425 for (int i = 0; i < nTracks; ++i) {
90 for (auto mit = mQuInd.begin(); mit != mQuInd.end(); ++mit) {
91 int i = mit->second;
92 CbmStsTrack* st = (CbmStsTrack*)fInputTracks->UncheckedAt(i);
93 Int_t nHits = st->GetNStsHits();
94 // if (nHits < 5) continue; // AZ-010625
95 // if (st->GetB() < -0.1) continue; // AZ-220525 - using TMVA for fake rejection
96 if (nHits < 4)
97 continue;
98 if (st->GetFlag())
99 continue;
100 if (st->GetChi2() < 0. || st->GetChi2() > c2cut * st->GetNDF())
101 continue;
102 // if (1/TMath::Abs(st->GetParamFirst()->GetQp()) < 0.2) continue; //AZ-151023 - exclude low momenta
103 // AZ-270425 CbmKFTrack& T = cloneArray[i];
104 CbmKFTrack& T = cloneArray[iii++]; // AZ-270425
105 T.SetStsTrack(*st);
106 T.Extrapolate(12.0); // AZ-161023 - go closer to the target
107 if (!finite(T.GetTrack()[0]) || !finite(T.GetCovMatrix()[0]))
108 continue;
109 T.SetTrkID(i); // AZ-090824
110 AddTrack(&T);
111 }
112
113 EvalVertex();
114 if (GetBreakout())
115 return; // AZ-010625 - for plots
116 CbmKFVertex v;
117 Fit(v);
118 v.GetVertex(*fPrimVert);
119 // Chi2Vertex();
120 fPrimVert->trkID = fTrkID; // AZ-090824
121
122 // FillVertex(); // fill vertex info
123
124 // AZ-300825 Flag tracks in vertex
125 int inVtx = fTrkID.size();
126
127 for (int j = 0; j < inVtx; ++j) {
128 int indx = fTrkID[j];
129 CbmStsTrack* st = (CbmStsTrack*)fInputTracks->UncheckedAt(indx);
130 st->SetFlag(st->GetFlag() | 2); // flag track in vertex
131 }
132
133 delete[] cloneArray;
134}
135
136//__________________________________________________________________________
137
138void BmnKFPrimaryVertexFinder::Clear()
139{
140 fTracks.clear();
141 for (int j = 0; j < 2; ++j) {
142 fHist[j]->Reset();
143 fHistw[j]->Reset();
144 fHistErr[j]->Reset();
145 }
146 fTrkID.clear(); // AZ-090824
147
148 // AZ-300525
149 fHist3->Reset();
150 fHistw3->Reset();
151 fHistZ->Reset();
152 fHZR->Reset();
153 fHistR->Reset();
154}
155
156//__________________________________________________________________________
157
158void BmnKFPrimaryVertexFinder::AddTrack(CbmKFTrackInterface* Track)
159{
160 fTracks.push_back(Track);
161}
162
163//__________________________________________________________________________
164
165void BmnKFPrimaryVertexFinder::SetTracks(vector<CbmKFTrackInterface*>& vTr)
166{
167 fTracks = vTr;
168}
169
170//__________________________________________________________________________
171
172void BmnKFPrimaryVertexFinder::EvalVertex()
173{
174 // Evaluate X and Y coordinates of vertex at Z = Ztarg
175
176 int nTracks = fTracks.size();
177 if (nTracks == 0)
178 return;
179 Double_t ztarg = 0.0, sigmas[2] = {0};
180
181 if (!CbmKF::Instance()->vTargets.empty()) {
182 CbmKFTube& t = CbmKF::Instance()->vTargets[0];
183 ztarg = t.z;
184 }
185 for (int i = 0; i < 2; ++i) {
186 fHist[i]->SetAxisRange(10, 0); // reset axes range
187 fHistw[i]->SetAxisRange(10, 0); // reset axes range
188 }
189
190 //---> AZ-300525
191 int nz = fHist3->GetNbinsZ();
192
193 for (int itr = 0; itr < nTracks; ++itr) {
194 CbmKFTrack tr(*fTracks[itr]);
195 CbmStsTrack* ststr = (CbmStsTrack*)fInputTracks->UncheckedAt(fTracks[itr]->GetTrkID());
196 Double_t www = ststr->GetNStsHits();
197
198 for (int iz = nz; iz >= 1; --iz) {
199 Double_t z = fHist3->GetZaxis()->GetBinCenter(iz);
200 Bool_t err = tr.Extrapolate(z);
201 if (err)
202 continue;
203 Double_t xtr = tr.GetTrack()[0];
204 Double_t ytr = tr.GetTrack()[1];
205 fHist3->Fill(xtr, ytr, z, 1);
206 fHistw3->Fill(xtr, ytr, z, www);
207 if (TMath::Abs(xtr) < 2.5 && TMath::Abs(ytr) < 2.5)
208 fHistZ->Fill(z);
209 fHZR->Fill(z, TMath::Sqrt(xtr * xtr + ytr * ytr), 1);
210 }
211 }
212 TH2D* hxy = (TH2D*)fHist3->Project3D("yx");
213 TH2D* hzx = (TH2D*)fHist3->Project3D("xz");
214 TH2D* hzy = (TH2D*)fHist3->Project3D("yz");
215 // Find Z-position
216 int ibmax = fHistZ->GetMaximumBin(), nmax = 0;
217 Double_t wind = fHistZ->GetBinWidth(1) * 5;
218 Double_t zmin = fHistZ->GetBinCenter(ibmax) - wind;
219 Double_t zmax = fHistZ->GetBinCenter(ibmax) + wind;
220 if (fHistZ->GetMaximum() > 0.5) {
221 // auto fitRes = fHistZ->Fit("gaus","sw","",zmin,zmax);
222 auto fitRes = fHistZ->Fit("gaus", "sw0q", "", zmin, zmax);
223 Double_t peakint = fHistZ->Integral(TMath::Max(1, ibmax - 5), TMath::Min(nz, ibmax + 5));
224 Double_t zerr = fitRes->GetParams()[2] / TMath::Sqrt(peakint);
225 Double_t zpeak = fitRes->GetParams()[1];
226 fMean[2] = 999;
227 if (TMath::Abs(zpeak - ztarg) > 3 * zerr) { // 3*sigma
228 // Check peak in ZY-histogram
229 hzy->GetXaxis()->SetRange(ibmax - 5, ibmax + 5);
230 if (hzy->GetMaximum() > 2) {
231 int ix, iy, iz;
232 hzy->GetMaximumBin(ix, iy, iz);
233 zpeak = fHistZ->GetBinCenter(ix);
234 if (TMath::Abs(zpeak - ztarg) > 3 * zerr) {
235 fMean[2] = zpeak;
236 ztarg = fMean[2];
237 }
238 }
239 hzy->GetXaxis()->SetRange(2, 1); // reset range
240 }
241 fSigma[2] = zerr;
242 // cout << zpeak << " " << zerr << " " << ztarg << endl;
243 // Find maxima
244 if (fHistZ->GetMaximum() > 2) {
245 ++nmax;
246 int ib = 1, ie = ibmax, idi = 1, cmax = -1;
247 if (nz - ibmax > ie - 1) {
248 ib = nz;
249 idi = -1;
250 }
251 Double_t cmax2 = 0.25 * fHistZ->GetMaximum(); // minimum value of 2'nd maximum
252 cmax2 = TMath::Max(cmax2, 2.0);
253
254 for (int j = ib; j != ie; j += idi) {
255 int cont = fHistZ->GetBinContent(j);
256 if (cont > cmax2 && cont > cmax)
257 cmax = cont;
258 else if (cont < cmax * 0.75) {
259 ++nmax;
260 break;
261 }
262 }
263 }
264 } // if (fHistZ->GetMaximum() > 0.5)
265 //<--- AZ-300525
266 fPrimVert->SetUniqueID(nmax);
267 vector<Double_t> vecDist(nTracks, 999);
268
269 for (int itr = 0; itr < nTracks; ++itr) {
270 CbmKFTrack tr(*fTracks[itr]);
271 Bool_t err = tr.Extrapolate(ztarg);
272 if (err)
273 continue;
274 // AZ-240525 Double_t wx = 1 / TMath::Sqrt(tr.GetCovMatrix()[0]);
275 // AZ-240525 Double_t wy = 1 / TMath::Sqrt(tr.GetCovMatrix()[2]);
276 // cout << itr << " xxx " << fTracks[itr]->GetTrkID() << endl;
277 CbmStsTrack* ststr = (CbmStsTrack*)fInputTracks->UncheckedAt(
278 fTracks[itr]->GetTrkID()); // AZ-240525
279 // AZ-020725 Double_t wx = ststr->GetNStsHits(); // AZ-240525
280 // AZ-020725 Double_t wy = wx; // AZ-240525
281 Double_t wx = 1 / ststr->GetParamFirst()->GetCovariance(0, 0); // AZ-020725
282 Double_t wy = 1 / ststr->GetParamFirst()->GetCovariance(1, 1); // AZ-020725
283 // cout << wx << " -------- " << wy << endl;
284 Double_t xtr = tr.GetTrack()[0];
285 Double_t ytr = tr.GetTrack()[1];
286 fHist[0]->Fill(xtr);
287 fHist[1]->Fill(ytr);
288 fHistw[0]->Fill(xtr, wx);
289 fHistw[1]->Fill(ytr, wy);
290 fHistErr[0]->Fill(TMath::Sqrt(tr.GetCovMatrix()[0])); // AZ-240524
291 fHistErr[1]->Fill(TMath::Sqrt(tr.GetCovMatrix()[2]));
292 Double_t dist = TMath::Sqrt(xtr * xtr + ytr * ytr);
293 fHistR->Fill(dist);
294 vecDist[itr] = dist;
295 }
296 // AZ-200226 Double_t peakDist = fHistR->GetBinCenter(fHistR->GetMaximumBin());
297 //---> AZ-280426 - procedure to avoid "circular" artefacts (take mean of 5 bins)
298 int irmax = fHistR->GetMaximumBin(), nbins = 0;
299 int i1 = irmax - 2, i2 = irmax + 2;
300 if (i1 < 1) {
301 i1 = 1;
302 i2 = i1 + 4;
303 }
304 if (i2 > fHistR->GetNbinsX()) {
305 i2 = fHistR->GetNbinsX();
306 i1 = i2 - 4;
307 }
308
309 // Take mean of 5 bins
310 Double_t wr = 0.0, wsum = 0.0, peakDist = 0.0;
311
312 for (int iii = i1; iii <= i2; ++iii) {
313 Double_t w = fHistR->GetBinContent(iii);
314 wsum += w;
315 wr += (w * fHistR->GetBinCenter(iii));
316 if (w > 0.1)
317 ++nbins;
318 }
319 if (nbins > 1)
320 peakDist = wr / wsum;
321 else
322 peakDist = fHistR->GetBinLowEdge(irmax) + gRandom->Rndm() * fHistR->GetBinWidth(irmax);
323 //<--- AZ
324
325 // Reshuffle tracks according to their distance to the mean value
326 multimap<Double_t, int> mapDist;
327
328 for (int itr = 0; itr < nTracks; ++itr)
329 mapDist.insert(pair<Double_t, int>(TMath::Abs(vecDist[itr] - peakDist), itr));
330
331 vector<CbmKFTrackInterface*> vTracks;
332
333 for (auto mit = mapDist.begin(); mit != mapDist.end(); ++mit) {
334 if (mit->first > 99)
335 break;
336 vTracks.push_back(fTracks[mit->second]);
337 }
338 // cout << " Tracks: " << vTracks.size() << " " << fTracks.size() << endl;
339 fTracks.clear();
340 fTracks = vTracks;
341
342 fSigma[0] = fSigma[1] = -1;
343
344 for (int ipass = 0; ipass < 2; ++ipass) {
345 /*cout << " EvalVertex: " << fHistw[0]->GetMean() << " " << fHist[0]->GetMaximum() << " "
346 << fHistw[1]->GetMean() << " " << fHist[1]->GetMaximum() << " " << fHist[0]->GetEntries() << endl;*/
347
348 for (int i = 0; i < 2; ++i) {
349 sigmas[i] = fHistw[i]->GetRMS();
350 if (sigmas[i] < 0.001) {
351 // fSigma[i] = 0.001;
352 fMean[i] = fHistw[i]->GetBinCenter(fHistw[i]->GetMaximumBin());
353 continue;
354 }
355 // cout << " EvalVertex: " << sigmas[i] << endl;
356 int sum = fHist[i]->GetEffectiveEntries();
357 // cout << i << " " << sum << " " << fHist[i]->GetMaximum() << endl;
358 if (sum > fHist[i]->GetMaximum()) {
359 // More than 1 bin
360 fSigma[i] = sigmas[i]; // AZ-400426
361 // fSigma[i] = fHistw[i]->GetMeanError(); // AZ-240525
362 // AZ-240525 fMean[i] = fHistw[i]->GetMean();
363 fMean[i] = fHistw[i]->GetBinCenter(fHistw[i]->GetMaximumBin()); // AZ-240525
364 }
365 }
366
367 if (ipass == 0) {
368 for (int i = 0; i < 2; ++i) {
369 if (sigmas[i] < 0.001)
370 continue;
371 Double_t xpeak = fHistw[i]->GetBinCenter(fHistw[i]->GetMaximumBin());
372 Double_t xmin = xpeak - sigmas[i];
373 Double_t xmax = xpeak + sigmas[i];
374 fHistw[i]->SetAxisRange(xmin, xmax); // change axis range
375 fHist[i]->SetAxisRange(xmin, xmax); // change axis range
376 }
377 }
378 }
379 TCanvas* c1 = (TCanvas*)gROOT->FindObject("cv1");
380 if (c1) {
381 for (int j = 0; j < 6; ++j) {
382 if (j == 0) {
383 fHistZ->Draw();
384 } else if (j == 1)
385 hxy->Draw("col");
386 else if (j == 2)
387 hzx->Draw("col");
388 else if (j == 3)
389 hzy->Draw("col");
390 else if (j == 4)
391 fHZR->Draw("col");
392 else if (j == 5) {
393 TAxis* zax = fHist3->GetZaxis();
394 int nbz = zax->GetNbins();
395 int iz0 = zax->FindBin(0.0);
396 zax->SetRange(iz0, iz0);
397 TH2D* hhh = (TH2D*)fHist3->Project3D("yx");
398 hhh->Draw("col");
399 zax->SetRange(1, nbz);
400 }
401 c1->Modified();
402 c1->Update();
403 if (getchar() == 'e') { // AZ-010625 - for plots
404 SetBreakout(1);
405 return;
406 }
407 }
408 for (int i = 0; i < 2; ++i) {
409 // fHist[i]->Draw();
410 fHistw[i]->Draw();
411 c1->Modified();
412 c1->Update();
413 getchar();
414 }
415 } // if (c1)
416}
417
418//__________________________________________________________________________
419
420void BmnKFPrimaryVertexFinder::Fit(CbmKFVertexInterface& vtx)
421{
422 //* Constants
423
424 // const Double_t CutChi2 = 3.5*3.5;
425 const Int_t MaxIter = 3; // 3;
426 // const Double_t chi2acc[3] = {10., 15., 20.};
427 const Double_t chi2acc[3] = {10., 12.25, 12.25};
428
429 //* Vertex state vector and the covariance matrix
430
431 Double_t r[3], *C = vtx.GetCovMatrix();
432
433 //* Initialize the vertex
434
435 for (Int_t i = 0; i < 6; i++)
436 C[i] = 0;
437 if (CbmKF::Instance()->vTargets.empty()) {
438 r[0] = r[1] = r[2] = 0.;
439 C[0] = C[2] = 5.;
440 C[5] = 0.25;
441 } else {
442 CbmKFTube& t = CbmKF::Instance()->vTargets[0];
443 r[0] = r[1] = 0.;
444 r[2] = t.z;
445 // AZ-151023 C[0] = C[2] = t.RR/3.5/3.5;
446 C[0] = C[2] = 2 * t.RR / 3.5 / 3.5; // AZ-151023
447 // AZ-181023 C[5] = (t.dz/2/3.5)*(t.dz/2/3.5);
448 C[5] = (t.dz / 3.5) * (t.dz / 3.5); // AZ-181023
449 r[0] = 0.4; // AZ-210923 - run 8
450 // cout << t.x << " " << t.y << " " << t.z << endl; //AZ-180923
451 // cout << t.RR << " " << t.dz << endl; //AZ-180923
452 // exit(0); //AZ-180923
453 }
454 // Take evaluated parameters
455 if (fSigma[0] > 0) {
456 r[0] = fMean[0];
457 C[0] = fSigma[0] * fSigma[0];
458 }
459 if (fSigma[1] > 0) {
460 r[1] = fMean[1];
461 C[2] = fSigma[1] * fSigma[1];
462 }
463 // AZ-310525
464 if (fSigma[2] > 0) {
465 if (fMean[2] < 900) {
466 r[2] = fMean[2];
467 C[5] = TMath::Max(fSigma[2] * fSigma[2], C[5]);
468 }
469 }
470
471 //* Iteratively fit vertex - number of iterations fixed at MaxIter
472 TMatrixD c(3, 3), cov(3, 3), xk0(3, 1), xk(3, 1), ck0(5, 1), a(5, 3), b(5, 3);
473 TMatrixD g(5, 5);
474 int ok = 1, nOK = 0, nTracks = fTracks.size();
475 Double_t chi2 = 0.;
476
477 xk.SetMatrixArray(r);
478 // cout << " Tracks: " << nTracks << " " << xk(0, 0) << " " << xk(1, 0) << " " << xk(2, 0) << endl;
479
480 for (Int_t iteration = 0; iteration < MaxIter; ++iteration) {
481
482 //* Store vertex from previous iteration
483
484 Double_t r0[3], C0[6];
485
486 for (Int_t i = 0; i < 3; i++)
487 r0[i] = r[i];
488 for (Int_t i = 0; i < 6; i++)
489 C0[i] = C[i];
490
491 //* Initialize the vertex covariance, Chi^2 & NDF
492
493 /*
494 C[0] = 100.;
495 C[1] = 0.; C[2] = 100.;
496 C[3] = 0.; C[4] = 0.; C[5] = 100.;
497 */
498
499 // AZ-210923 vtx.GetRefNDF() = -3;
500 vtx.GetRefNDF() = 0; // AZ-210923
501 vtx.GetRefChi2() = 0.;
502 vtx.GetRefNTracks() = 0;
503
504 Double_t cutChi2 = chi2acc[TMath::Min(iteration, 2)];
505 chi2 = 0.0;
506
507 if (iteration == 0) {
508 for (int i = 0; i < 3; ++i) {
509 for (int j = i; j < 3; ++j) {
510 int indx = CbmKFMath::indexS(i, j);
511 c(i, j) = c(j, i) = C0[indx];
512 }
513 }
514 cov = TMatrixD(TMatrixD::kInverted, c);
515 }
516
517 // AZ-010625 Int_t ibeg = nTracks - 1, iend = -1, istep = -1;
518 Int_t ibeg = 0, iend = nTracks, istep = 1; // AZ-010625
519 /* AZ-270425
520 if (iteration % 2 > 0) {
521 ibeg = 0;
522 iend = nTracks;
523 istep = 1;
524 }
525 */
526 nOK = 0;
527
528 // for (vector<CbmKFTrackInterface*>::iterator itr = fTracks.begin(); itr != fTracks.end() ; ++itr) {
529 for (int itr = ibeg; itr != iend; itr += istep) {
530
531 if (ok) {
532 xk0 = xk; // xk0 stands for x(k-1)
533 cov = TMatrixD(TMatrixD::kInverted, c);
534 }
535
536 // CbmKFTrack T(**itr);
537 CbmKFTrack T(*fTracks[itr]);
538 // AZ-131023 Bool_t err = T.Extrapolate( r0[2] );
539 // AZ-131023 if( err ) continue;
540
541 // Double_t *m = T.GetTrack();
542 Double_t* V = T.GetCovMatrix();
543 std::vector<Double_t> covMatr(V, V + 15); // AZ-190923
544 if (iteration == 0) {
545 for (unsigned int j = 0; j < covMatr.size(); ++j)
546 covMatr[j] *= 10;
547 } // AZ-190923
548 else
549 {
550 // AZ-240525 - restrict X-ccordinate accuracy to 300 um
551 Double_t errx = TMath::Sqrt(covMatr[0]);
552 if (errx < -0.01) {
553 Double_t scale = 0.01 / errx;
554 scale *= scale;
555 for (unsigned int j = 0; j < covMatr.size(); ++j)
556 covMatr[j] *= scale;
557 }
558 }
559 V = covMatr.data(); // AZ-190923
560
561 // Set g-matrix
562 for (int i = 0; i < 5; ++i) {
563 // for (int j = 0; j < 5; ++j) {
564 for (int j = i; j < 5; ++j) {
565 int indx = CbmKFMath::indexS(i, j);
566 g(i, j) = g(j, i) = V[indx];
567 }
568 }
569 // g.Print();
570 g.Invert();
571 // g.Print();
572 Bool_t err = T.Extrapolate(r0[2]);
573 if (err)
574 continue;
575 // cout << " --- xxx --- " << itr << " " << (T.GetRefNDF()+5)/2 << endl;
576
577 // ComputeAandB(xk, track, track1, a, b, ck0); // compute matrices of derivatives
578 // ComputeAandB (xk, T, (*itr)->GetTrack()[5], a, b, ck0); // compute matrices of derivatives
579 err = ComputeAandB(xk, T, fTracks[itr]->GetTrack()[5], a, b, ck0); // compute matrices of derivatives
580 if (err)
581 continue;
582
583 // W = (Bt*G*B)'
584 TMatrixD tmp(g, TMatrixD::kMult, b);
585 TMatrixD w(b, TMatrixD::kTransposeMult, tmp);
586 w.Invert();
587 // w.Print();
588
589 // Gb = G - G*B*W*Bt*G
590 TMatrixD tmp1(b, TMatrixD::kTransposeMult, g);
591 TMatrixD tmp2(w, TMatrixD::kMult, tmp1);
592 TMatrixD tmp3(b, TMatrixD::kMult, tmp2);
593 TMatrixD tmp4(g, TMatrixD::kMult, tmp3);
594 TMatrixD gb = g;
595 gb -= tmp4;
596
597 // Ck = ((Ck-1)' + At*Gb*A)'
598 TMatrixD tmp5(gb, TMatrixD::kMult, a);
599 c = TMatrixD(a, TMatrixD::kTransposeMult, tmp5);
600 // cout << "gb, cov, c" << endl;
601 // gb.Print();
602 // cov.Print();
603 // c.Print();
604 c += cov;
605 c.Invert();
606
607 // xk = Ck*((Ck-1)'x(k-1)+At*Gb*(m-ck0))
608 // TMatrixD m = *track1.GetParamNew();
609 TMatrixD m(5, 1);
610 // m.SetMatrixArray ((*itr)->GetTrack());
611 m.SetMatrixArray(fTracks[itr]->GetTrack());
612 m -= ck0; // m-ck0
613 // cout << " m-ck0: " << endl;
614 // ck0.Print();
615 // m.Print();
616 TMatrixD tmp11(gb, TMatrixD::kMult, m);
617 TMatrixD tmp12(a, TMatrixD::kTransposeMult, tmp11);
618 TMatrixD tmp13(cov, TMatrixD::kMult, xk0);
619 tmp13 += tmp12;
620 xk = TMatrixD(c, TMatrixD::kMult, tmp13);
621 // cout << " Vertex: " << itr << " " << track->GetTrackID() << " " << xk(0,0) << " " << xk(1,0) << " " <<
622 // xk(2,0) << endl;
623
624 // qk = W*Bt*G*(m-ck0-A*xk)
625 TMatrixD tmp21(a, TMatrixD::kMult, xk);
626 tmp21 *= -1;
627 tmp21 += m; // m-ck0-A*xk
628 TMatrixD tmp22(g, TMatrixD::kMult, tmp21);
629 TMatrixD tmp23(b, TMatrixD::kTransposeMult, tmp22);
630 TMatrixD qk(w, TMatrixD::kMult, tmp23);
631
632 // Residual m-ck0-A*xk-B*qk
633 TMatrixD res = tmp21;
634 TMatrixD tmp31(b, TMatrixD::kMult, qk);
635 res -= tmp31;
636 // cout << " Residual matrix: " << endl;
637 // res.Print();
638 // qk.Print();
639
640 // Chi2 = rt*G*res + (xk-x(k-1))t*(Ck-1)'*(xk-x(k-1))
641 TMatrixD tmp41(g, TMatrixD::kMult, res);
642 TMatrixD chi21(res, TMatrixD::kTransposeMult, tmp41);
643 // chi21.Print();
644 TMatrixD dx = xk;
645 dx -= xk0;
646 // dx.Print();
647 TMatrixD tmp42(cov, TMatrixD::kMult, dx);
648 TMatrixD chi22(dx, TMatrixD::kTransposeMult, tmp42);
649 // chi22.Print();
650 if (chi21(0, 0) < 0 || chi22(0, 0) < 0) {
651 cout << " FindVertex: chi2 < 0 " << chi21(0, 0) << " " << chi22(0, 0) << " " << iteration << " "
652 << endl;
653 // cout << track->GetNode() << " " << track->GetNodeNew() << " " << track->GetUniqueID() << " "
654 // << track->Momentum() << " " << track->GetDirection() << endl;
655 // exit(0);
656 chi21(0, 0) = TMath::Max(chi21(0, 0), 0.);
657 chi22(0, 0) = TMath::Max(chi22(0, 0), 0.);
658 }
659 chi21 += chi22;
660 if (chi21(0, 0) > cutChi2) {
661 ok = 0;
662 continue;
663 } // skip track
664 chi2 += chi21(0, 0);
665 ++nOK;
666 ok = 1;
667 if (iteration == MaxIter - 1)
668 fTrkID.push_back(fTracks[itr]->GetTrkID()); // AZ-090824
669 } // for (int itr = ibeg; itr != iend;
670 /*
671 if (ok)
672 cout << " Vertex position: " << iteration << " " << xk(0, 0) << " " << xk(1, 0) << " " << xk(2, 0) << " " <<
673 chi2
674 << " " << nOK << " " << nPrim << endl;
675 else
676 cout << " Vertex position: " << iteration << " " << xk0(0, 0) << " " << xk0(1, 0) << " " << xk0(2, 0) << " "
677 << chi2 << " " << nOK << " " << nPrim << endl;
678 */
679
680 if (iteration < MaxIter - 1) { // AZ-181023 - reduce target-Z constraint
681 if (ok)
682 c *= 4;
683 else
684 cov *= 0.25;
685 }
686
687 } // for (Int_t iteration = 0; iteration < MaxIter;
688
689 if (ok == 0) {
690 // Take vertex after last accepted track
691 c = TMatrixD(TMatrixD::kInverted, cov);
692 xk = xk0;
693 }
694 /*
695 Double_t a = 0, b = 0;
696 {
697 Double_t zeta[2] = { r0[0]-m[0], r0[1]-m[1] };
698
699 // Check track Chi^2 deviation from the r0 vertex estimate
700
701 //AZ-190923 Double_t S[3] = { (C0[2]+V[2]), -(C0[1]+V[1]), (C0[0]+V[0]) };
702 Double_t S[3] = { (C0[0]+V[0]), -(C0[1]+V[1]), (C0[2]+V[2]) }; //AZ-190923
703 Double_t s = S[2]*S[0] - S[1]*S[1];
704 Double_t chi2 = zeta[0]*zeta[0]*S[0] + 2*zeta[0]*zeta[1]*S[1]
705 + zeta[1]*zeta[1]*S[2];
706 if( chi2 > s*CutChi2 ) continue;
707
708 // Fit of vertex track slopes (a,b) to r0 vertex
709
710 s = V[0]*V[2] - V[1]*V[1];
711 if ( s < 1.E-20 ) continue;
712 s = 1./s;
713 a = m[2] + s*( ( V[3]*V[2] - V[4]*V[1] )*zeta[0]
714 + (- V[3]*V[1] + V[4]*V[0] )*zeta[1] );
715 b = m[3] + s*( ( V[6]*V[2] - V[7]*V[1] )*zeta[0]
716 + (- V[6]*V[1] + V[7]*V[0] )*zeta[1] );
717 }
718
719 // Update the vertex (r,C) with the track estimate (m,V) :
720
721 // Linearized measurement matrix H = { { 1, 0, -a}, { 0, 1, -b} };
722
723 // Residual (measured - estimated)
724
725 Double_t zeta[2] = { m[0] - ( r[0] - a*(r[2]-r0[2]) ),
726 m[1] - ( r[1] - b*(r[2]-r0[2]) ) };
727
728 // CHt = CH'
729
730 Double_t CHt[3][2] = { { C[0] - a*C[3], C[1] - b*C[3]},
731 { C[1] - a*C[4], C[2] - b*C[4]},
732 { C[3] - a*C[5], C[4] - b*C[5]} };
733
734 // S = (H*C*H' + V )^{-1}
735
736 Double_t S[3] = { V[0] + CHt[0][0] - a*CHt[2][0],
737 V[1] + CHt[1][0] - b*CHt[2][0],
738 V[2] + CHt[1][1] - b*CHt[2][1] };
739
740 // Invert S
741 {
742 Double_t w = S[0]*S[2] - S[1]*S[1];
743 if ( w < 1.E-20 ) continue;
744 w = 1./w;
745 Double_t S0 = S[0];
746 S[0] = w*S[2];
747 S[1] = -w*S[1];
748 S[2] = w*S0;
749 }
750
751 // Calculate Chi^2
752
753 vtx.GetRefChi2()+= zeta[0]*zeta[0]*S[0] + 2*zeta[0]*zeta[1]*S[1]
754 //AZ-190923 + zeta[1]*zeta[1]*S[2] + T.GetRefChi2();
755 + zeta[1]*zeta[1]*S[2]; //AZ-190923
756 //AZ-190923 vtx.GetRefNDF() += 2 + T.GetRefNDF();
757 vtx.GetRefNDF() += 2; //AZ-190923
758 vtx.GetRefNTracks()++;
759
760 // Kalman gain K = CH'*S
761
762 Double_t K[3][2];
763
764 for( Int_t i=0; i<3; ++i ){ K[i][0] = CHt[i][0]*S[0] + CHt[i][1]*S[1] ;
765 K[i][1] = CHt[i][0]*S[1] + CHt[i][1]*S[2] ; }
766
767 // New estimation of the vertex position r += K*zeta
768
769 for( Int_t i=0; i<3; ++i ) r[i]+= K[i][0]*zeta[0] + K[i][1]*zeta[1];
770
771 // New covariance matrix C -= K*(CH')'
772
773 C[0] -= K[0][0]*CHt[0][0] + K[0][1]*CHt[0][1];
774 C[1] -= K[1][0]*CHt[0][0] + K[1][1]*CHt[0][1];
775 C[2] -= K[1][0]*CHt[1][0] + K[1][1]*CHt[1][1];
776 C[3] -= K[2][0]*CHt[0][0] + K[2][1]*CHt[0][1];
777 C[4] -= K[2][0]*CHt[1][0] + K[2][1]*CHt[1][1];
778 C[5] -= K[2][0]*CHt[2][0] + K[2][1]*CHt[2][1];
779
780 }// itr
781
782 }// finished iterations
783 */
784
785 // Copy state vector to output (covariance matrix was used directly )
786
787 // vtx.GetRefX() = r[0];
788 // vtx.GetRefY() = r[1];
789 // vtx.GetRefZ() = r[2];
790 vtx.GetRefX() = xk(0, 0);
791 vtx.GetRefY() = xk(1, 0);
792 vtx.GetRefZ() = xk(2, 0);
793 vtx.GetRefNTracks() = nOK;
794 vtx.GetRefChi2() = chi2;
795 vtx.GetRefNDF() = nOK * 2;
796
797 for (int i = 0; i < 3; ++i) {
798 for (int j = i; j < 3; ++j) {
799 int indx = CbmKFMath::indexS(i, j);
800 C[indx] = c(i, j);
801 }
802 }
803}
804
805//__________________________________________________________________________
806
807// void BmnKFPrimaryVertexFinder::ComputeAandB(TMatrixD &xk0, const MpdKalmanTrack *track, const MpdKalmanTrack &trackM,
808// TMatrixD &a, TMatrixD &b, TMatrixD &ck0)
809// void BmnKFPrimaryVertexFinder::ComputeAandB(TMatrixD &xk0, const CbmKFTrack &track, Double_t zhit, TMatrixD &a,
810// TMatrixD &b, TMatrixD &ck0)
811Bool_t BmnKFPrimaryVertexFinder::ComputeAandB(TMatrixD& xk0,
812 CbmKFTrackInterface& track,
813 Double_t zhit,
814 TMatrixD& a,
815 TMatrixD& b,
816 TMatrixD& ck0)
817{
819 // (track - at dca to (0,0), trackM - at TPC inner shell - for Proxim)
820
821 Double_t vert0[3], *vert = xk0.GetMatrixArray();
822
823 for (Int_t i = 0; i < 3; ++i)
824 vert0[i] = vert[i];
825
826 /*AZ-121023 - doesn't seem to be necessary
827 MpdKalmanTrack trackk = *track;
828 trackk.SetPos(trackk.GetPosNew());
829 // trackk.GetParam()->Print();
830 // Propagate track to PCA w.r.t. point xk0
831 MpdKalmanFilter::Instance()->FindPca(&trackk, vert0);
832 // MpdKalmanFilter::Instance()->FindPca(&trackk,zero); // just for test
833 // cout << trackk.GetPosNew() << endl;
834 trackk.SetParam(*trackk.GetParamNew());
835 // trackk.GetParam()->Print();
836 */
837 // Find PCA to vert0
838 // CbmKFTrack trackk(track);
839 // FindPca (trackk, vert0);
840
841 // Put track at xk0
842 CbmKFTrack tr(track);
843 // CbmKFTrack tr(trackk);
844 tr.GetTrack()[0] = vert0[0];
845 tr.GetTrack()[1] = vert0[1];
846 tr.GetTrack()[5] = vert0[2];
847 CbmKFTrack tr0(tr);
848
849 // Propagate track to zhit
850 Bool_t err = tr.Extrapolate(zhit);
851 // if (err) continue;
852 if (err)
853 return err;
854
855 // Double_t shift = 0.01; // 100 um coordinate shift
856 Double_t shift = 0.1; // 1 mm coordinate shift
857
858 for (Int_t i = 0; i < 3; ++i) {
859 // MpdKalmanTrack track1 = track0;
860 CbmKFTrack tr1(tr0);
861 vert0[i] += shift;
862 if (i > 0)
863 vert0[i - 1] -= shift;
864 tr1.GetTrack()[0] = vert0[0];
865 tr1.GetTrack()[1] = vert0[1];
866 tr1.GetTrack()[5] = vert0[2];
867
868 // Propagate track to zhit
869 err = tr1.Extrapolate(zhit);
870 // if (err) continue;
871 if (err)
872 return err;
873
874 // Derivatives
875 for (Int_t j = 0; j < 5; ++j) {
876 // a(j, i) = (track1.GetParamNew(j) - trackk.GetParamNew(j)) / shift;
877 a(j, i) = (tr1.GetTrack()[j] - tr.GetTrack()[j]) / shift;
878 }
879 }
880
881 for (Int_t i = 0; i < 3; ++i) {
882 // MpdKalmanTrack track1 = track0;
883 CbmKFTrack tr1(tr0);
884 Int_t j = i + 2;
885 // shift = (*track->GetCovariance())(j, j);
886 int indx = CbmKFMath::indexS(j, j);
887 shift = track.GetCovMatrix()[indx];
888 shift = TMath::Sqrt(shift);
889 if (j == 4)
890 shift *= TMath::Sign(1., -tr0.GetTrack()[j]); // 1/p
891 // track1.SetParam(j, track0.GetParamNew(j) + shift);
892 tr1.GetTrack()[j] += shift;
893
894 // Propagate track to zhit
895 err = tr1.Extrapolate(zhit);
896 // if (err) continue;
897 if (err)
898 return err;
899
900 // Derivatives
901 for (Int_t k = 0; k < 5; ++k) {
902 // b(k, i) = (track1.GetParamNew(k) - trackk.GetParamNew(k)) / shift;
903 b(k, i) = (tr1.GetTrack()[k] - tr.GetTrack()[k]) / shift;
904 }
905 }
906
907 TMatrixD qk0(3, 1);
908 // for (Int_t i = 0; i < 3; ++i) qk0(i, 0) = track0.GetParamNew(i + 2);
909 for (Int_t i = 0; i < 3; ++i)
910 qk0(i, 0) = tr0.GetTrack()[i + 2];
911 // qk0.Print();
912 // ck0 = *trackk.GetParamNew();
913 for (int i = 0; i < 5; ++i)
914 ck0(i, 0) = tr.GetTrack()[i];
915 ck0 -= TMatrixD(a, TMatrixD::kMult, xk0);
916 ck0 -= TMatrixD(b, TMatrixD::kMult, qk0);
917 // cout << " Derivatives: " << endl;
918 // a.Print();
919 // b.Print();
920 // ck0.Print();
921 // TMatrixD(b,TMatrixD::kMult,qk0).Print();
922 // TMatrixD(a,TMatrixD::kMult,xk0).Print();
923 return kFALSE;
924}
925
926//__________________________________________________________________________
927
928void BmnKFPrimaryVertexFinder::FindPca(CbmKFTrackInterface& tr, Double_t* vert)
929{
930 // Find track PCA w.r.t. vert using parabolic approximation
931
932 // CbmKFTrack tr(track);
933 fHistPca->Reset();
934
935 for (int i = -1; i < 2; ++i) {
936 tr.Extrapolate(vert[2] + 1.0 * i);
937 Double_t* pars = tr.GetTrack();
938 Double_t dx = pars[0] - vert[0], dy = pars[1] - vert[1], dz = pars[5] - vert[2];
939 Double_t dist = TMath::Sqrt(dx * dx + dy * dy + dz * dz);
940 fHistPca->Fill(dz, dist);
941 }
942 fHistPca->Fit("pol2", "Q0");
943 TF1* func = fHistPca->GetFunction("pol2");
944 Double_t dzPca = func->GetMinimumX();
945 // cout << dzPca << endl;
946 tr.Extrapolate(vert[2] + dzPca);
947
948 // TCanvas *c1 = (TCanvas*) gROOT->FindObject("c1");
949 // if (c1) { fHistPca->Draw(); c1->Modified(); c1->Update(); getchar(); }
950}
dist
vector< Double_t > dist(vector< Double_t > qp, Double_t mu)
Definition BmnMath.cxx:869
v
__m128 v
Definition P4_F32vec4.h:1
i
int i
Definition P4_F32vec4.h:22
m
__m128 m
Definition P4_F32vec4.h:27
BmnKFPrimaryVertexFinder::AddTrack
void AddTrack(CbmKFTrackInterface *Track)
Definition BmnKFPrimaryVertexFinder.cxx:158
BmnKFPrimaryVertexFinder::SetTracks
void SetTracks(std::vector< CbmKFTrackInterface * > &vTracks)
Definition BmnKFPrimaryVertexFinder.cxx:165
BmnKFPrimaryVertexFinder::ComputeAandB
Bool_t ComputeAandB(TMatrixD &xk0, CbmKFTrackInterface &track, Double_t zhit, TMatrixD &a, TMatrixD &b, TMatrixD &ck0)
Definition BmnKFPrimaryVertexFinder.cxx:811
BmnKFPrimaryVertexFinder::FindPca
void FindPca(CbmKFTrackInterface &track, Double_t *vert)
Definition BmnKFPrimaryVertexFinder.cxx:928
BmnKFPrimaryVertexFinder::Fit
void Fit(CbmKFVertexInterface &vtx)
Definition BmnKFPrimaryVertexFinder.cxx:420
CbmKFMath::indexS
static Int_t indexS(Int_t i, Int_t j)
Definition CbmKFMath.h:33
CbmKFTrackInterface::GetTrack
virtual Double_t * GetTrack()
Is it electron.
Definition CbmKFTrackInterface.cxx:31
CbmKFTrackInterface::Extrapolate
Int_t Extrapolate(Double_t z, Double_t *QP0=0, Bool_t line=false)
Access to i-th hit.
Definition CbmKFTrackInterface.cxx:37
CbmKFTrackInterface::GetCovMatrix
virtual Double_t * GetCovMatrix()
array[6] of track parameters(x,y,tx,ty,qp,z)
Definition CbmKFTrackInterface.cxx:32
CbmKFTrack::SetStsTrack
void SetStsTrack(CbmStsTrack &track, bool first=1)
Definition CbmKFTrack.cxx:33
CbmKFTrack::GetTrack
Double_t * GetTrack()
Is it electron.
Definition CbmKFTrack.h:51
CbmKFTrack::GetCovMatrix
Double_t * GetCovMatrix()
array[6] of track parameters(x,y,tx,ty,qp,z)
Definition CbmKFTrack.h:52
CbmKFTube::RR
Double_t RR
Definition CbmKFMaterial.h:86
CbmKFTube::dz
Double_t dz
Definition CbmKFMaterial.h:85
CbmKFTube::z
Double_t z
Definition CbmKFMaterial.h:85
CbmKFVertexInterface::GetRefY
virtual Double_t & GetRefY()
Definition CbmKFVertexInterface.cxx:28
CbmKFVertexInterface::GetRefX
virtual Double_t & GetRefX()
Definition CbmKFVertexInterface.cxx:24
CbmKFVertexInterface::GetRefNTracks
virtual Int_t & GetRefNTracks()
Number of Degrees of Freedom after fit.
Definition CbmKFVertexInterface.cxx:48
CbmKFVertexInterface::GetRefZ
virtual Double_t & GetRefZ()
Definition CbmKFVertexInterface.cxx:32
CbmKFVertexInterface::GetRefChi2
virtual Double_t & GetRefChi2()
Array[6] of covariance matrix.
Definition CbmKFVertexInterface.cxx:40
CbmKFVertexInterface::GetRefNDF
virtual Int_t & GetRefNDF()
Chi^2 after fit.
Definition CbmKFVertexInterface.cxx:44
CbmKFVertexInterface::GetCovMatrix
virtual Double_t * GetCovMatrix()
Definition CbmKFVertexInterface.cxx:36
CbmKF::Instance
static CbmKF * Instance()
Definition CbmKF.h:35
CbmKF::vTargets
std::vector< CbmKFTube > vTargets
Definition CbmKF.h:63
CbmStsTrack::GetFlag
Int_t GetFlag() const
Definition CbmStsTrack.h:65
CbmStsTrack::SetFlag
void SetFlag(Int_t flag)
Definition CbmStsTrack.h:85
CbmStsTrack::GetNStsHits
Int_t GetNStsHits() const
Definition CbmStsTrack.h:60
CbmStsTrack::GetChi2
Double_t GetChi2() const
Definition CbmStsTrack.h:66
CbmStsTrack::GetParamFirst
FairTrackParam * GetParamFirst()
Definition CbmStsTrack.h:69
CbmStsTrack::GetNDF
Int_t GetNDF() const
Definition CbmStsTrack.h:67
CbmVertex::trkID
std::vector< Int_t > trkID
Definition CbmVertex.h:67