MpdKfPrimaryVertexFinder.cxx 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857
  1. // -------------------------------------------------------------------------
  2. // ----- MpdKfPrimaryVertexFinder source file -----
  3. // ----- Created 4/07/09 by A. Zinchenko -----
  4. // -------------------------------------------------------------------------
  5. /** MpdKfPrimaryVertexFinder.h
  6. *@author A.Zinchenko <Alexander.Zinchenko@jinr.ru>
  7. **
  8. ** Primary vertex finder in MPD based on the Kalman filter
  9. **/
  10. #include "MpdKfPrimaryVertexFinder.h"
  11. #include "MpdKalmanFilter.h"
  12. #include "MpdKalmanTrack.h"
  13. #include "MpdKalmanGeoScheme.h"
  14. #include "MpdKalmanHit.h"
  15. #include "MpdTpcKalmanTrack.h"
  16. #include "MpdVertex.h"
  17. #include "FairMCPoint.h"
  18. #include "MpdMCTrack.h"
  19. #include "FairRootManager.h"
  20. #include <TAxis.h>
  21. #include <TClonesArray.h>
  22. #include <TF1.h>
  23. #include <TH1D.h>
  24. #include <TMath.h>
  25. #include <TMatrixD.h>
  26. #include <TMatrixFSym.h>
  27. #include <TRandom.h>
  28. #include <TFile.h>
  29. #include <iostream>
  30. #include <vector>
  31. using std::cout;
  32. using std::endl;
  33. using std::vector;
  34. const Double_t MpdKfPrimaryVertexFinder::fgkChi2Cut = 3.5 * 3.5;
  35. FILE *lunVtx = 0x0; //fopen("vtx.dat","w");
  36. //__________________________________________________________________________
  37. MpdKfPrimaryVertexFinder::MpdKfPrimaryVertexFinder(const char *name, Int_t iVerbose)
  38. :FairTask(name, iVerbose), fConstrFlag(0), fSmoothSame(0), fVertTracks(nullptr)
  39. {
  40. fHistoDir = nullptr;
  41. fHist[0] = fHist[1] = fHist[2] = nullptr;
  42. fCovar.ResizeTo(3,3);
  43. for (Int_t i = 0; i < 3; ++i) fXYZ[i] = 0;
  44. fUnc = nullptr;
  45. }
  46. //__________________________________________________________________________
  47. MpdKfPrimaryVertexFinder::~MpdKfPrimaryVertexFinder()
  48. {
  49. //delete fKHits;
  50. //delete fTracks;
  51. //delete fTrackCand;
  52. for (Int_t i = 0; i < 3; ++i)
  53. if (fHist[i] != nullptr) fHist[i]->Delete();
  54. delete fUnc;
  55. }
  56. //__________________________________________________________________________
  57. InitStatus MpdKfPrimaryVertexFinder::Init()
  58. {
  59. fVertexCont = new TClonesArray("MpdVertex", 5);
  60. if (fConstrFlag) fVertTracks = new TClonesArray("MpdTpcKalmanTrack");
  61. fUnc = new TF1("fUnc","[0]*exp(-0.5*(x-[1])*(x-[1])/[2]/[2])",-5,5);
  62. fHist[0] = new TH1D("hXv","Xv",40,-2.05,1.95);
  63. fHist[1] = new TH1D("hYv","Yv",40,-2.05,1.95);
  64. fHist[2] = new TH1D("hZv","Zv",1000,-100.1,99.9);
  65. fTracks = 0x0;
  66. fTracks = (TClonesArray *) FairRootManager::Instance()->GetObject("TpcTrackRefit");
  67. if (fTracks == 0x0) fTracks = (TClonesArray *) FairRootManager::Instance()->GetObject("ItsTrack");
  68. if (fTracks == 0x0) fTracks = (TClonesArray *) FairRootManager::Instance()->GetObject("ItsTrackKF");
  69. if (fTracks == 0x0) fTracks = (TClonesArray *) FairRootManager::Instance()->GetObject("TpcKalmanTrack");
  70. fMCTracks =(TClonesArray *) FairRootManager::Instance()->GetObject("MCTrack");
  71. FairRootManager::Instance()->Register("Vertex", "MPDVertex", fVertexCont, kTRUE);
  72. if (fConstrFlag) FairRootManager::Instance()->Register("PrimTracks", "ConstrTrs", fVertTracks, kTRUE);
  73. fNPass = 3;
  74. return kSUCCESS;
  75. }
  76. //__________________________________________________________________________
  77. InitStatus MpdKfPrimaryVertexFinder::ReInit()
  78. {
  79. return kSUCCESS;
  80. }
  81. //__________________________________________________________________________
  82. void MpdKfPrimaryVertexFinder::Reset()
  83. {
  84. ///
  85. cout << " MpdKfPrimaryVertexFinder::Reset " << endl;
  86. if (fVertexCont) fVertexCont->Delete();
  87. if (fConstrFlag) fVertTracks->Delete();
  88. }
  89. //__________________________________________________________________________
  90. void MpdKfPrimaryVertexFinder::SetParContainers()
  91. {
  92. }
  93. //__________________________________________________________________________
  94. void MpdKfPrimaryVertexFinder::Finish()
  95. {
  96. //Write();
  97. }
  98. //__________________________________________________________________________
  99. void MpdKfPrimaryVertexFinder::Exec(Option_t * option)
  100. {
  101. static int eventCounter = 0;
  102. cout << " VertexFinder event " << ++eventCounter << endl;
  103. Reset();
  104. EvalVertex();
  105. FindVertex();
  106. if (fConstrFlag) Smooth();
  107. Chi2Vertex();
  108. FillVertex(); // fill vertex info
  109. }
  110. //__________________________________________________________________________
  111. void MpdKfPrimaryVertexFinder::EvalVertex()
  112. {
  113. /// Primary vertex position evaluator
  114. Int_t nTracks = fTracks->GetEntriesFast();
  115. Double_t dMax = 0., dMin = 0.1, xyzM[3], xyzF[3] = {0,0,0}, xyz[3] = {0};
  116. TAxis *axis[3];
  117. for (Int_t i = 0; i < 3; ++i) {
  118. axis[i] = fHist[i]->GetXaxis();
  119. axis[i]->SetRange(10,0); // reset axes ranges
  120. //AZ xyz[i] = fXYZ[i];
  121. }
  122. fCovar = 0.;
  123. // Repeat until shift in transverse position becomes small
  124. for (Int_t iter = 0; iter < 5; ++iter) {
  125. for (Int_t i = 0; i < 3; ++i) fHist[i]->Reset();
  126. for (Int_t itr = 0; itr < nTracks; ++itr) {
  127. MpdKalmanTrack *track = (MpdKalmanTrack*) fTracks->UncheckedAt(itr);
  128. MpdKalmanTrack tr = *track;
  129. //if (iter > 0) {
  130. // Find new PCA
  131. tr.SetParamNew(*tr.GetParam());
  132. tr.SetPos(tr.GetPosNew());
  133. //track1.ReSetWeight();
  134. MpdKalmanFilter::Instance()->FindPca(&tr,xyz);
  135. tr.SetParam(*tr.GetParamNew());
  136. //}
  137. Double_t r = tr.GetPosNew();
  138. //cout << " pos " << iter << " " << itr << " " << r << endl;
  139. //tr.GetParam()->Print();
  140. //Double_t phi = 0;
  141. Double_t phi = tr.GetParam(2);
  142. if (r > 1.e-7) phi = tr.GetParam(0) / r;
  143. fHist[0]->Fill (r*TMath::Cos(phi)); // x of PCA to (0,0,0)
  144. fHist[1]->Fill (r*TMath::Sin(phi));
  145. fHist[2]->Fill (tr.GetParam(1));
  146. }
  147. dMax = 0.;
  148. for (Int_t i = 0; i < 3; ++i) {
  149. // Take mean value
  150. xyzM[i] = fHist[i]->GetMean();
  151. //AZ fHist[i]->SetAxisRange(xyzM[i]-2.,xyzM[i]+2.);
  152. fHist[i]->SetAxisRange(xyzM[i]-5.,xyzM[i]+5.);
  153. xyzM[i] = fHist[i]->GetMean(); // restricted mean
  154. if (fHist[i]->GetMaximum() > 0) xyzM[i] = fHist[i]->GetBinCenter(fHist[i]->GetMaximumBin()); // peak
  155. Double_t rms = TMath::Min (fHist[i]->GetRMS(), fHist[i]->GetMeanError() * 3);
  156. /*
  157. fUnc->SetParameters(fHist[i]->GetMaximum(),xyzM[i],rms/2);
  158. fUnc->SetParLimits(0,fHist[i]->GetMaximum()*0.8,fHist[i]->GetMaximum()*9);
  159. fUnc->SetParLimits(1,xyzM[i]-1.,xyzM[i]+1.);
  160. //fUnc->SetParLimits(2,0.0001,rms);
  161. fHist[i]->Fit(fUnc,"w","",xyzM[i]-2.,xyzM[i]+2.);
  162. xyzF[i] = fUnc->GetParameter(1);
  163. Double_t sigma = TMath::Abs (fUnc->GetParameter(2));
  164. */
  165. Double_t sigma = 999;
  166. if (sigma < rms) {
  167. xyz[i] = xyzF[i];
  168. fCovar(i,i) = sigma*sigma;
  169. }
  170. else {
  171. xyz[i] = xyzM[i];
  172. fCovar(i,i) = rms*rms;
  173. }
  174. cout << " Hist " << i << " " << fHist[i]->GetMaximum() << " " << xyzM[i] << " " << xyzF[i] << " " << rms << " " << sigma << endl;
  175. if (i == 2) continue;
  176. Double_t d = TMath::Abs (xyz[i]-fXYZ[i]);
  177. if (d > dMax) dMax = d;
  178. }
  179. for (Int_t i = 0; i < 3; ++i) fXYZ[i] = xyz[i];
  180. cout << iter << " " << dMax << endl;
  181. /*
  182. for (Int_t i = 0; i < 3; ++i) {
  183. fHist[i]->Draw();
  184. gPad->Update();
  185. Char_t go[1];
  186. gets(go);
  187. }
  188. */
  189. if (dMax < dMin) break;
  190. } // for (Int_t iter = 0; iter < 5;
  191. for (Int_t i = 0; i < 3; ++i) fCovar(i,i) = TMath::Max (fCovar(i,i),0.02*0.02);
  192. // Use beam tolerances
  193. Double_t sigx = 9, sigy = 9, sigxy = 0.02; // sigma 200 um
  194. while (sigx > 3*sigxy || sigy > 3*sigxy)
  195. gRandom->Rannor(sigx,sigy);
  196. fXYZ[0] = sigx * sigxy; // position
  197. fCovar(0,0) = sigxy*sigxy;
  198. fXYZ[1] = sigy * sigxy; // position
  199. fCovar(1,1) = sigxy*sigxy;
  200. }
  201. //__________________________________________________________________________
  202. void MpdKfPrimaryVertexFinder::FindVertex()
  203. {
  204. /// Kalman primary vertex finder
  205. //AZ-040121 const Double_t chi2acc[3] = {35., 25., 15.};
  206. const Double_t chi2acc[3] = {10., 15., 20.};
  207. Int_t nTracks = fTracks->GetEntriesFast(), nOK = 0, ok = 1;
  208. MpdVertex *vtx = new ((*fVertexCont)[0]) MpdVertex();
  209. TMatrixD c(3,3), cov(3,3), xk0(3,1), xk(3,1), ck0(5,1);
  210. TMatrixD a(5,3), b(5,3);
  211. MpdKalmanHit hit;
  212. Double_t chi2 = 0;
  213. //xk.Zero();
  214. //xk = 0.5;
  215. xk.SetMatrixArray(fXYZ);
  216. cout << " Tracks: " << nTracks << " " << xk(0,0) << " " << xk(1,0) << " " << xk(2,0) << endl;
  217. //hit.SetR(35.); // 4 cm
  218. //xk(0,0) = 1;
  219. //cov(0,0) = cov(1,1) = 1;
  220. //cov(2,2) = 25 * 25;
  221. vector<Int_t> inds;
  222. for (Int_t ipass = 0; ipass < fNPass; ++ipass) {
  223. chi2 = 0.;
  224. Double_t cutChi2 = chi2acc[TMath::Min(ipass,2)];
  225. //c.Zero();
  226. //c(0,0) = c(1,1) = c(2,2) = 100.;
  227. //c = fCovar;
  228. //cov = TMatrixD(TMatrixD::kInverted,c);
  229. if (ipass == 0) {
  230. c = fCovar;
  231. cov = TMatrixD(TMatrixD::kInverted,c);
  232. }
  233. //AZ-040121 Int_t nPrim = 0, ibeg = 0, iend = nTracks, istep = 1;
  234. //if (ipass % 2 > 0) { ibeg = nTracks-1; iend = -1; istep = -1; }
  235. Int_t nPrim = 0, ibeg = nTracks-1, iend = -1, istep = -1;
  236. if (ipass % 2 > 0) { ibeg = 0; iend = nTracks; istep = 1; }
  237. nOK = 0;
  238. for (Int_t itr = ibeg; itr != iend; itr+=istep) {
  239. //for (Int_t itr = 0; itr < nTracks; ++itr) {
  240. //for (Int_t itr = nTracks-1; itr > -1; --itr) {
  241. if (ok) {
  242. xk0 = xk; // xk0 stands for x(k-1)
  243. cov = TMatrixD(TMatrixD::kInverted,c);
  244. }
  245. MpdKalmanTrack *track = (MpdKalmanTrack*) fTracks->UncheckedAt(itr);
  246. //if (track->GetNode() != "") continue; // exclude failed tracks
  247. // Select primaries
  248. //MpdMCTrack *mcTr = (MpdMCTrack*) fMCTracks->UncheckedAt(track->GetTrackID());
  249. //if (mcTr->GetMotherId() >= 0) continue; // secondary
  250. //Double_t th = TMath::PiOver2() - track->GetParam(3);
  251. //if (TMath::Abs(TMath::Log(TMath::Tan(th/2))) > 1.) continue; // eta-cut
  252. //if (1./TMath::Abs(track->GetParam(4)) < 0.2) continue; // pt-cut
  253. ++nPrim;
  254. MpdKalmanTrack track1 = *track;
  255. track1.SetParamNew(*track1.GetParam());
  256. track1.SetPos(track1.GetPosNew());
  257. track1.ReSetWeight();
  258. TMatrixD g = *track1.GetWeight(); // track weight matrix
  259. if (ipass == 0 && track->GetUniqueID()) g *= 0.04; //AZ-050121 - downweight ITS track
  260. //track1.GetWeight()->Print();
  261. if (track->GetNode() == "") {
  262. hit.SetType(MpdKalmanHit::kFixedR);
  263. hit.SetPos(track->GetPos());
  264. } else {
  265. // 03-jan-2018 - skip track not pointing to the beam line
  266. ok = 0;
  267. continue;
  268. hit.SetType(MpdKalmanHit::kFixedP);
  269. TString detName = track->GetNode();
  270. if (track->GetUniqueID()) {
  271. // ITS
  272. detName = detName(16,detName.Length());
  273. detName += "#0";
  274. }
  275. MpdKalmanGeoScheme *geo = MpdKalmanFilter::Instance()->GetGeo();
  276. hit.SetDetectorID(geo->DetId(detName));
  277. // Find distance from the current track position to the last point (plane) -
  278. // to define direction (mainly for ITS)
  279. TVector3 pos = geo->GlobalPos(&hit);
  280. TVector3 norm = geo->Normal(&hit);
  281. Double_t v7[7] = {0.0};
  282. TString node = track1.GetNode();
  283. track1.SetNode("");
  284. MpdKalmanFilter::Instance()->SetGeantParamB(&track1, v7, 1);
  285. Double_t d = -(pos * norm); // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  286. TVector3 v3(v7[0], v7[1], v7[2]);
  287. d += v3 * norm;
  288. if (d < 0) track1.SetDirection(MpdKalmanTrack::kOutward);
  289. }
  290. MpdKalmanFilter::Instance()->PropagateToHit(&track1,&hit,kFALSE,kTRUE);
  291. //track1.GetParamNew()->Print();
  292. //cout << " Meas. radius " << track1.GetPosNew() << endl;
  293. //TMatrixD g = *track1.GetWeight(); // track weight matrix
  294. //track->GetCovariance()->Print();
  295. //track1.GetCovariance()->Print();
  296. //g.Print();
  297. //track1.Weight2Cov();
  298. //track1.GetCovariance()->Print();
  299. ComputeAandB(xk0,track,track1,a,b,ck0); // compute matrices of derivatives
  300. // W = (Bt*G*B)'
  301. TMatrixD tmp(g,TMatrixD::kMult,b);
  302. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  303. w.Invert();
  304. // Gb = G - G*B*W*Bt*G
  305. TMatrixD tmp1(b,TMatrixD::kTransposeMult,g);
  306. TMatrixD tmp2(w,TMatrixD::kMult,tmp1);
  307. TMatrixD tmp3(b,TMatrixD::kMult,tmp2);
  308. TMatrixD tmp4(g,TMatrixD::kMult,tmp3);
  309. TMatrixD gb = g;
  310. gb -= tmp4;
  311. // Ck = ((Ck-1)' + At*Gb*A)'
  312. TMatrixD tmp5(gb,TMatrixD::kMult,a);
  313. c = TMatrixD(a,TMatrixD::kTransposeMult,tmp5);
  314. //cout << "gb, cov, c" << endl;
  315. //gb.Print();
  316. //cov.Print();
  317. //c.Print();
  318. c += cov;
  319. c.Invert();
  320. // xk = Ck*((Ck-1)'x(k-1)+At*Gb*(m-ck0))
  321. TMatrixD m = *track1.GetParamNew();
  322. m -= ck0; // m-ck0
  323. //cout << " m-ck0: " << endl;
  324. //ck0.Print();
  325. //m.Print();
  326. TMatrixD tmp11(gb,TMatrixD::kMult,m);
  327. TMatrixD tmp12(a,TMatrixD::kTransposeMult,tmp11);
  328. TMatrixD tmp13(cov,TMatrixD::kMult,xk0);
  329. tmp13 += tmp12;
  330. xk = TMatrixD(c,TMatrixD::kMult,tmp13);
  331. //cout << " Vertex: " << itr << " " << track->GetTrackID() << " " << xk(0,0) << " " << xk(1,0) << " " << xk(2,0) << endl;
  332. // qk = W*Bt*G*(m-ck0-A*xk)
  333. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  334. tmp21 *= -1;
  335. tmp21 += m; // m-ck0-A*xk
  336. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  337. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  338. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  339. // Residual m-ck0-A*xk-B*qk
  340. TMatrixD r = tmp21;
  341. TMatrixD tmp31(b,TMatrixD::kMult,qk);
  342. r -= tmp31;
  343. //cout << " Residual matrix: " << endl;
  344. //r.Print();
  345. //qk.Print();
  346. // Chi2 = rt*G*r + (xk-x(k-1))t*(Ck-1)'*(xk-x(k-1))
  347. TMatrixD tmp41(g,TMatrixD::kMult,r);
  348. TMatrixD chi21(r,TMatrixD::kTransposeMult,tmp41);
  349. //chi21.Print();
  350. TMatrixD dx = xk;
  351. dx -= xk0;
  352. //dx.Print();
  353. TMatrixD tmp42(cov,TMatrixD::kMult,dx);
  354. TMatrixD chi22(dx,TMatrixD::kTransposeMult,tmp42);
  355. //chi22.Print();
  356. if (chi21(0,0) < 0 || chi22(0,0) < 0) {
  357. cout << " FindVertex: chi2 < 0 " << chi21(0,0) << " " << chi22(0,0) << " " << ipass << " " << itr << endl;
  358. cout << track->GetNode() << " " << track->GetNodeNew() << " " << track->GetUniqueID() << " " << track->Momentum() << " " << track->GetDirection() << endl;
  359. //exit(0);
  360. chi21(0,0) = TMath::Max (chi21(0,0),0.);
  361. chi22(0,0) = TMath::Max (chi22(0,0),0.);
  362. }
  363. chi21 += chi22;
  364. if (chi21(0,0) > cutChi2) { ok = 0; continue; } // skip track
  365. chi2 += chi21(0,0);
  366. ++nOK;
  367. ok = 1;
  368. if (ipass == fNPass-1) inds.push_back(itr);
  369. //if (lunVtx) fprintf(lunVtx,"%10.3e \n",chi21(0,0));
  370. //chi21.Print();
  371. //c.Print();
  372. //TMatrixD www = c;
  373. //www.Invert();
  374. //www.Print();
  375. } // for (Int_t itr = 0; itr < nTracks;
  376. if (ok) cout << " Vertex position: " << ipass << " " << xk(0,0) << " " << xk(1,0) << " " << xk(2,0) << " " << chi2 << " " << nOK << " " << nPrim << endl;
  377. else cout << " Vertex position: " << ipass << " " << xk0(0,0) << " " << xk0(1,0) << " " << xk0(2,0) << " " << chi2 << " " << nOK << " " << nPrim << endl;
  378. //fCovar = c;
  379. //xk.GetMatrix2Array(fXYZ);
  380. } // for (Int_t ipass = 0; ipass < fNPass;
  381. if (ok == 0) {
  382. // Take vertex after last accepted track
  383. c = TMatrixD(TMatrixD::kInverted,cov);
  384. xk = xk0;
  385. }
  386. TMatrixFSym covar(3);
  387. for (Int_t i = 0; i < 3; ++i) {
  388. for (Int_t j = 0; j < 3; ++j) covar(i,j) = c(i,j); }
  389. vtx->SetVertex(xk(0,0),xk(1,0),xk(2,0),chi2,2*nOK,nOK,covar);
  390. TArrayI *ind = vtx->GetIndices();
  391. ind->Set(nOK);
  392. for (Int_t i = 0; i < nOK; ++i) (*ind)[i] = inds[i];
  393. fCovar = c; // save for further use
  394. if (lunVtx) {
  395. for (Int_t i = 0; i < 3; ++i) {
  396. for (Int_t j = i; j < 3; ++j) fprintf(lunVtx,"%10.3e ",covar(i,j)); }
  397. fprintf(lunVtx,"\n");
  398. }//*/
  399. }
  400. //__________________________________________________________________________
  401. void MpdKfPrimaryVertexFinder::Smooth()
  402. {
  403. /// Smooth tracks from primary vertex (update momentum and track length -
  404. /// covariance matrix is not updated !!!)
  405. MpdVertex *vtx = (MpdVertex*) fVertexCont->UncheckedAt(0);
  406. TArrayI *ind = vtx->GetIndices();
  407. Int_t nPrim = ind->GetSize();
  408. MpdKalmanHit hit;
  409. TMatrixD c(3,3), xk(3,1), ck0(5,1);
  410. TMatrixD a(5,3), b(5,3);
  411. TVector3 vert;
  412. vtx->Position(vert);
  413. xk(0,0) = vert.X();
  414. xk(1,0) = vert.Y();
  415. xk(2,0) = vert.Z();
  416. Double_t rad = vert.Pt();
  417. for (Int_t itr = 0; itr < nPrim; ++itr) {
  418. MpdKalmanTrack *track = (MpdKalmanTrack*) fTracks->UncheckedAt((*ind)[itr]);
  419. MpdKalmanTrack *trVert = nullptr;
  420. if (fConstrFlag) trVert =
  421. new((*fVertTracks)[itr]) MpdTpcKalmanTrack(*(MpdTpcKalmanTrack*)track);
  422. MpdKalmanTrack track1 = *track;
  423. track1.SetParamNew(*track1.GetParam());
  424. track1.SetPos(track1.GetPosNew());
  425. track1.ReSetWeight();
  426. track1.SetLength(0.);
  427. TMatrixD g = *track1.GetWeight(); // track weight matrix
  428. //track1.GetWeight()->Print();
  429. if (track->GetNode() == "") {
  430. hit.SetType(MpdKalmanHit::kFixedR);
  431. hit.SetPos(track->GetPos());
  432. } else {
  433. hit.SetType(MpdKalmanHit::kFixedP);
  434. TString detName = track->GetNode();
  435. if (track->GetUniqueID()) {
  436. // ITS
  437. detName = detName(16,detName.Length());
  438. detName += "#0";
  439. }
  440. MpdKalmanGeoScheme *geo = MpdKalmanFilter::Instance()->GetGeo();
  441. hit.SetDetectorID(geo->DetId(detName));
  442. // Find distance from the current track position to the last point (plane) -
  443. // to define direction (mainly for ITS)
  444. TVector3 pos = geo->GlobalPos(&hit);
  445. TVector3 norm = geo->Normal(&hit);
  446. Double_t v7[7] = {0.0};
  447. track1.SetNode("");
  448. MpdKalmanFilter::Instance()->SetGeantParamB(&track1, v7, 1);
  449. Double_t d = -(pos * norm); // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  450. TVector3 v3(v7[0], v7[1], v7[2]);
  451. d += v3 * norm;
  452. if (d < 0) track1.SetDirection(MpdKalmanTrack::kOutward);
  453. }
  454. MpdKalmanFilter::Instance()->PropagateToHit(&track1,&hit,kTRUE,kTRUE);
  455. //track1.GetParamNew()->Print();
  456. //cout << " Meas. radius " << track1.GetPosNew() << endl;
  457. ComputeAandB(xk,track,track1,a,b,ck0); // compute matrices of derivatives
  458. // W = (Bt*G*B)'
  459. TMatrixD tmp(g,TMatrixD::kMult,b);
  460. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  461. w.Invert();
  462. TMatrixD m = *track1.GetParamNew();
  463. m -= ck0; // m-ck0
  464. // qk = W*Bt*G*(m-ck0-A*xk)
  465. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  466. tmp21 *= -1;
  467. tmp21 += m; // m-ck0-A*xk
  468. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  469. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  470. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  471. // Update momentum and last coordinates
  472. TMatrixD par = *track->GetParamNew();
  473. for (Int_t i = 0; i < 3; ++i) par(i+2,0) = qk(i,0);
  474. par(0,0) = rad * vert.Phi();
  475. par(1,0) = vert.Z();
  476. if (trVert) {
  477. trVert->SetParam(par);
  478. trVert->SetPosNew(rad);
  479. } else if (fSmoothSame) {
  480. track->SetParam(par);
  481. track->SetPosNew(rad);
  482. }
  483. // Update track length
  484. Double_t dLeng = track1.GetLength(); // track length from DCA to last saved position
  485. track1.SetParam(par);
  486. track1.SetPos(rad);
  487. track1.SetLength(0.);
  488. if (track->GetNode() == "") MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kTRUE);
  489. else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kTRUE,kTRUE);
  490. if (trVert) trVert->SetLength (track->GetLength() - dLeng + track1.GetLength());
  491. else if (fSmoothSame) track->SetLength (track->GetLength() - dLeng + track1.GetLength());
  492. } // for (Int_t itr = 0; itr < nPrim;
  493. }
  494. //__________________________________________________________________________
  495. void MpdKfPrimaryVertexFinder::ComputeAandB(TMatrixD &xk0, const MpdKalmanTrack *track,
  496. const MpdKalmanTrack &trackM,
  497. TMatrixD &a, TMatrixD &b, TMatrixD &ck0)
  498. {
  499. /// Compute matrices of derivatives w.r.t. vertex coordinates and track momentum
  500. Double_t vert0[3], *vert = xk0.GetMatrixArray(); //zero[3] = {0},
  501. for (Int_t i = 0; i < 3; ++i) vert0[i] = vert[i];
  502. MpdKalmanTrack trackk = *track;
  503. trackk.SetPos(trackk.GetPosNew());
  504. //trackk.GetParam()->Print();
  505. // Propagate track to PCA w.r.t. point xk0
  506. MpdKalmanFilter::Instance()->FindPca(&trackk,vert0);
  507. //MpdKalmanFilter::Instance()->FindPca(&trackk,zero); // just for test
  508. //cout << trackk.GetPosNew() << endl;
  509. trackk.SetParam(*trackk.GetParamNew());
  510. //trackk.GetParam()->Print();
  511. // Put track at xk0
  512. Double_t r = TMath::Sqrt (vert0[0] * vert0[0] + vert0[1] * vert0[1]);
  513. Double_t phi = trackk.GetParamNew(2); // track Phi
  514. if (r > 1.e-7) phi = TMath::ATan2 (vert0[1], vert0[0]);
  515. trackk.SetPos(r);
  516. trackk.SetParam(0,r*phi);
  517. trackk.SetParam(1,vert0[2]);
  518. trackk.SetNode("");
  519. MpdKalmanTrack track0 = trackk;
  520. // Propagate track to chosen radius
  521. MpdKalmanHit hit;
  522. //hit.SetR(35.);
  523. //hit = *(MpdKalmanHitR*)track->GetTrHits()->Last();
  524. if (track->GetNode() == "") {
  525. hit.SetType(MpdKalmanHit::kFixedR);
  526. //hit.SetR(35.);
  527. //hit = *(MpdKalmanHitR*)track->GetTrHits()->Last();
  528. hit.SetPos(track->GetPos());
  529. MpdKalmanFilter::Instance()->PropagateParamR(&trackk,&hit,kFALSE);
  530. //trackk.GetParamNew()->Print();
  531. Proxim(trackM,trackk);
  532. } else {
  533. hit.SetType(MpdKalmanHit::kFixedP);
  534. TString detName = track->GetNode();
  535. if (track->GetUniqueID()) {
  536. // ITS
  537. detName = detName(16,detName.Length());
  538. detName += "#0";
  539. }
  540. MpdKalmanGeoScheme *geo = MpdKalmanFilter::Instance()->GetGeo();
  541. hit.SetDetectorID(geo->DetId(detName));
  542. // Find distance from the current track position to the last point (plane) -
  543. // to define direction (mainly for ITS)
  544. TVector3 pos = geo->GlobalPos(&hit);
  545. TVector3 norm = geo->Normal(&hit);
  546. Double_t v7[7] = {0.0};
  547. MpdKalmanFilter::Instance()->SetGeantParamB(&trackk, v7, 1);
  548. Double_t d = -(pos * norm); // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  549. TVector3 v3(v7[0], v7[1], v7[2]);
  550. d += v3 * norm;
  551. if (d < 0) trackk.SetDirection(MpdKalmanTrack::kOutward);
  552. MpdKalmanFilter::Instance()->PropagateParamP(&trackk,&hit,kFALSE,kTRUE);
  553. track0.SetDirection(trackk.GetDirection());
  554. }
  555. //Double_t shift = 0.01; // 100 um coordinate shift
  556. Double_t shift = 0.1; // 1 mm coordinate shift
  557. for (Int_t i = 0; i < 3; ++i) {
  558. MpdKalmanTrack track1 = track0;
  559. vert0[i] += shift;
  560. if (i > 0) vert0[i-1] -= shift;
  561. r = TMath::Sqrt (vert0[0] * vert0[0] + vert0[1] * vert0[1]);
  562. if (r > 1.e-7) phi = TMath::ATan2 (vert0[1], vert0[0]);
  563. else phi = track0.GetParamNew(2); // track Phi
  564. track1.SetPos(r);
  565. track1.SetParam(0,r*phi);
  566. track1.SetParam(1,vert0[2]);
  567. if (track->GetNode() == "") {
  568. MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kFALSE);
  569. Proxim(trackk,track1);
  570. //Proxim(track1,trackk);
  571. } else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kFALSE,kTRUE);
  572. // Derivatives
  573. for (Int_t j = 0; j < 5; ++j) {
  574. a(j,i) = (track1.GetParamNew(j) - trackk.GetParamNew(j)) / shift;
  575. }
  576. }
  577. for (Int_t i = 0; i < 3; ++i) {
  578. MpdKalmanTrack track1 = track0;
  579. Int_t j = i + 2;
  580. shift = (*track->GetCovariance())(j,j);
  581. shift = TMath::Sqrt(shift);
  582. if (j == 4) shift *= TMath::Sign(1.,-track0.GetParamNew(j)); // 1/p
  583. track1.SetParam(j,track0.GetParamNew(j)+shift);
  584. //if (j == 2 && track1.GetParamNew(j)*TMath::Sign(1.,track1.GetParamNew(j)) > TMath::Pi())
  585. //track1.SetParam(j,track0.GetParamNew(j)-shift);
  586. if (track->GetNode() == "") {
  587. MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kFALSE);
  588. Proxim(trackk,track1);
  589. //Proxim(track1,trackk);
  590. } else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kFALSE,kTRUE);
  591. // Derivatives
  592. for (Int_t k = 0; k < 5; ++k) {
  593. b(k,i) = (track1.GetParamNew(k) - trackk.GetParamNew(k)) / shift;
  594. }
  595. }
  596. TMatrixD qk0(3,1);
  597. for (Int_t i = 0; i < 3; ++i) qk0(i,0) = track0.GetParamNew(i+2);
  598. //qk0.Print();
  599. ck0 = *trackk.GetParamNew();
  600. ck0 -= TMatrixD(a,TMatrixD::kMult,xk0);
  601. ck0 -= TMatrixD(b,TMatrixD::kMult,qk0);
  602. //cout << " Derivatives: " << endl;
  603. //a.Print();
  604. //b.Print();
  605. //ck0.Print();
  606. //TMatrixD(b,TMatrixD::kMult,qk0).Print();
  607. //TMatrixD(a,TMatrixD::kMult,xk0).Print();
  608. }
  609. //__________________________________________________________________________
  610. void MpdKfPrimaryVertexFinder::Proxim(const MpdKalmanTrack &track0, MpdKalmanTrack &track)
  611. {
  612. /// Adjust track parameters
  613. if (track0.GetType() != MpdKalmanTrack::kBarrel) {
  614. cout << " !!! Implemented only for kBarrel tracks !!!" << endl;
  615. exit(0);
  616. }
  617. //Double_t tmp = track.GetParamNew(0);
  618. Double_t phi0 = track0.GetParamNew(0) / track0.GetPosNew();
  619. Double_t phi = track.GetParamNew(0) / track.GetPosNew();
  620. phi = MpdKalmanFilter::Instance()->Proxim(phi0,phi);
  621. TMatrixD *par = track.GetParamNew();
  622. (*par)(0,0) = phi * track.GetPosNew();
  623. phi0 = track0.GetParamNew(2);
  624. phi = track.GetParamNew(2);
  625. phi = MpdKalmanFilter::Instance()->Proxim(phi0,phi);
  626. (*par)(2,0) = phi;
  627. track.SetParamNew(*par);
  628. //cout << " Proxim: " << track0.GetParamNew(0) << " " << track.GetParamNew(0) << " " << tmp << endl;
  629. }
  630. //__________________________________________________________________________
  631. void MpdKfPrimaryVertexFinder::Write()
  632. {
  633. /// Write
  634. TFile histoFile("Vertex.root","RECREATE");
  635. Writedir2current(fHistoDir);
  636. histoFile.Close();
  637. }
  638. //__________________________________________________________________________
  639. void MpdKfPrimaryVertexFinder::Writedir2current( TObject *obj )
  640. {
  641. /// Write
  642. if( !obj->IsFolder() ) obj->Write();
  643. else{
  644. TDirectory *cur = gDirectory;
  645. TDirectory *sub = cur->mkdir(obj->GetName());
  646. sub->cd();
  647. TList *listSub = ((TDirectory*)obj)->GetList();
  648. TIter it(listSub);
  649. while( TObject *obj1=it() ) Writedir2current(obj1);
  650. cur->cd();
  651. }
  652. }
  653. //__________________________________________________________________________
  654. void MpdKfPrimaryVertexFinder::FillVertex()
  655. {
  656. /// Fill vertex info
  657. }
  658. //__________________________________________________________________________
  659. void MpdKfPrimaryVertexFinder::Chi2Vertex()
  660. {
  661. /// Compute Chi2-distance of tracks from the primary vertex
  662. Int_t nTracks = fTracks->GetEntriesFast(), nPrim = 0;
  663. MpdVertex *vtx = (MpdVertex*) fVertexCont->UncheckedAt(0);
  664. TMatrixD c(3,3), cov(3,3), xk0(3,1), xk(3,1), ck0(5,1);
  665. TMatrixD a(5,3), b(5,3);
  666. MpdKalmanHit hit;
  667. hit.SetType(MpdKalmanHit::kFixedR);
  668. TVector3 vert;
  669. vtx->Position(vert);
  670. xk0(0,0) = vert.X();
  671. xk0(1,0) = vert.Y();
  672. xk0(2,0) = vert.Z();
  673. //xk(0,0) = xk(1,0) = xk(2,0) = 0.;
  674. //cov = fCovar;
  675. TMatrixFSym covM(3);
  676. vtx->CovMatrix(covM);
  677. cov = covM;
  678. //AZ cov.Invert();
  679. Int_t iii = 0;
  680. MpdKalmanFilter::Instance()->MnvertLocal(cov.GetMatrixArray(), 3, 3, 3, iii);
  681. //vvv.Print();
  682. //fCovar.Print();
  683. for (Int_t itr = 0; itr < nTracks; ++itr) {
  684. MpdKalmanTrack *track = (MpdKalmanTrack*) fTracks->UncheckedAt(itr);
  685. if (track->GetNode() != "") {
  686. track->SetChi2Vertex(999.);
  687. continue; // exclude failed tracks
  688. }
  689. // Select primaries
  690. //MpdMCTrack *mcTr = (MpdMCTrack*) fMCTracks->UncheckedAt(track->GetTrackID());
  691. //if (mcTr->GetMotherId() >= 0) continue; // secondary
  692. //Double_t th = TMath::PiOver2() - track->GetParam(3);
  693. //if (TMath::Abs(TMath::Log(TMath::Tan(th/2))) > 1.) continue; // eta-cut
  694. //if (1./TMath::Abs(track->GetParam(4)) < 0.2) continue; // pt-cut
  695. ++nPrim;
  696. //hit = *(MpdKalmanHitR*)track->GetTrHits()->Last();
  697. //MpdKalmanFilter::Instance()->GetGeo()->SetGlobalPos(&hit,TVector3(track->GetPos(),0.,0.),kTRUE);
  698. hit.SetPos(track->GetPos());
  699. MpdKalmanTrack track1 = *track;
  700. track1.SetParamNew(*track1.GetParam());
  701. track1.SetPos(track1.GetPosNew());
  702. track1.ReSetWeight();
  703. TMatrixD g = *track1.GetWeight(); // track weight matrix
  704. MpdKalmanFilter::Instance()->PropagateToHit(&track1,&hit,kFALSE);
  705. ComputeAandB(xk0,track,track1,a,b,ck0); // compute matrices of derivatives
  706. // W = (Bt*G*B)'
  707. TMatrixD tmp(g,TMatrixD::kMult,b);
  708. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  709. w.Invert();
  710. // Gb = G - G*B*W*Bt*G
  711. TMatrixD tmp1(b,TMatrixD::kTransposeMult,g);
  712. TMatrixD tmp2(w,TMatrixD::kMult,tmp1);
  713. TMatrixD tmp3(b,TMatrixD::kMult,tmp2);
  714. TMatrixD tmp4(g,TMatrixD::kMult,tmp3);
  715. TMatrixD gb = g;
  716. gb -= tmp4;
  717. // Ck = ((Ck-1)' + At*Gb*A)'
  718. TMatrixD tmp5(gb,TMatrixD::kMult,a);
  719. c = TMatrixD(a,TMatrixD::kTransposeMult,tmp5);
  720. c += cov;
  721. c.Invert();
  722. // xk = Ck*((Ck-1)'x(k-1)+At*Gb*(m-ck0))
  723. TMatrixD m = *track1.GetParamNew();
  724. m -= ck0; // m-ck0
  725. TMatrixD tmp11(gb,TMatrixD::kMult,m);
  726. TMatrixD tmp12(a,TMatrixD::kTransposeMult,tmp11);
  727. TMatrixD tmp13(cov,TMatrixD::kMult,xk0);
  728. tmp13 += tmp12;
  729. xk = TMatrixD(c,TMatrixD::kMult,tmp13);
  730. // qk = W*Bt*G*(m-ck0-A*xk)
  731. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  732. tmp21 *= -1;
  733. tmp21 += m; // m-ck0-A*xk
  734. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  735. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  736. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  737. // Residual m-ck0-A*xk-B*qk
  738. TMatrixD r = tmp21;
  739. TMatrixD tmp31(b,TMatrixD::kMult,qk);
  740. r -= tmp31;
  741. // Chi2 = rt*G*r + (xk-x(k-1))t*(Ck-1)'*(xk-x(k-1))
  742. TMatrixD tmp41(g,TMatrixD::kMult,r);
  743. TMatrixD chi21(r,TMatrixD::kTransposeMult,tmp41);
  744. TMatrixD dx = xk;
  745. dx -= xk0;
  746. TMatrixD tmp42(cov,TMatrixD::kMult,dx);
  747. TMatrixD chi22(dx,TMatrixD::kTransposeMult,tmp42);
  748. //cout << " Chi2Vertex: " << chi21(0,0) << " " << chi22(0,0) << endl;
  749. chi21 += chi22;
  750. //chi21.Print();
  751. track->SetChi2Vertex(chi21(0,0));
  752. } // for (Int_t itr = 0; itr < nTracks;
  753. }
  754. ClassImp(MpdKfPrimaryVertexFinder);