MpdTpcKalmanTrack.cxx 9.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282
  1. /// \class MpdTpcKalmanTrack
  2. ///
  3. /// Kalman filter track object for the MPD central detector
  4. /// Track parameters: 0: RPhi - coordinate in R-Phi direction
  5. /// 1: Z - longitudinal coordinate
  6. /// 2: Phi - local azimuthal angle
  7. /// 3: Theta - local dip angle (angle w.r.t. the transverse plane)
  8. /// 4: q/Pt - signed inverse Pt
  9. /// \author Alexander Zinchenko (LHEP, JINR, Dubna)
  10. #include "MpdTpcKalmanTrack.h"
  11. #include "MpdKalmanFilter.h"
  12. #include "MpdKalmanGeoScheme.h"
  13. #include "MpdKalmanHit.h"
  14. #include "MpdKalmanTrack.h"
  15. //#include "MpdTpcHit.h" // debug
  16. //#include "TpcPoint.h" // debug
  17. #include <TMatrixD.h>
  18. #include <TMatrixDSym.h>
  19. #include <TClonesArray.h>
  20. #include <TVector3.h>
  21. #include <TMath.h>
  22. #include <Riostream.h>
  23. using namespace std;
  24. //__________________________________________________________________________
  25. MpdTpcKalmanTrack::MpdTpcKalmanTrack()
  26. : MpdKalmanTrack(),
  27. fTrHits(new TClonesArray("MpdKalmanHit"))
  28. {
  29. /// Default constructor
  30. }
  31. //__________________________________________________________________________
  32. MpdTpcKalmanTrack::MpdTpcKalmanTrack(MpdKalmanHit *hitOut, MpdKalmanHit *hitIn,
  33. TVector3 &vertex, Double_t pt, Double_t *params)
  34. : MpdKalmanTrack(0., vertex),
  35. fTrHits(new TClonesArray("MpdKalmanHit",70))
  36. {
  37. /// Constructor from 2 hits
  38. Double_t rOut = hitOut->GetPos();
  39. Double_t phiOut = hitOut->GetMeas(0) / rOut;
  40. Double_t rIn = hitIn->GetPos();
  41. Double_t phiIn = hitIn->GetMeas(0) / rIn;
  42. Double_t parOut[4] = {rOut, phiOut, 0., 0.};
  43. Double_t parIn[4] = {rIn, phiIn, 0., 0.};
  44. EvalParams(hitOut, hitIn, parOut, parIn, pt, params);
  45. }
  46. //__________________________________________________________________________
  47. MpdTpcKalmanTrack::MpdTpcKalmanTrack(MpdKalmanHit *hitOut, MpdKalmanHit *hitIn,
  48. TVector3 &vertex, TVector3 &posOut, TVector3 &posIn,
  49. Double_t pt, Double_t *params)
  50. : MpdKalmanTrack(0., vertex),
  51. fTrHits(new TClonesArray("MpdKalmanHit",70))
  52. {
  53. /// Constructor from 2 hits and 2 TVector3's (modular geometry)
  54. Double_t rOut = posOut.Pt(), phiOut = posOut.Phi();
  55. Double_t rIn = posIn.Pt(), phiIn = posIn.Phi();
  56. Double_t parOut[4] = {rOut, phiOut, 0., 0.};
  57. Double_t parIn[4] = {rIn, phiIn, 0., 0.};
  58. EvalParams(hitOut, hitIn, parOut, parIn, pt, params);
  59. }
  60. //__________________________________________________________________________
  61. MpdTpcKalmanTrack::MpdTpcKalmanTrack (const MpdTpcKalmanTrack& track)
  62. : MpdKalmanTrack(track),
  63. fTrHits(new TClonesArray("MpdKalmanHit",70))
  64. {
  65. ///copy constructor
  66. Int_t nHits = track.fTrHits->GetEntriesFast();
  67. for (Int_t i = 0; i < nHits; ++i) {
  68. MpdKalmanHit *hit = (MpdKalmanHit*) (track.fTrHits->UncheckedAt(i));
  69. new ((*fTrHits)[i]) MpdKalmanHit(*hit);
  70. }
  71. if (track.fHits == 0x0) {
  72. for (Int_t i = 0; i < nHits; ++i) {
  73. fHits->Add(fTrHits->UncheckedAt(i));
  74. }
  75. }
  76. }
  77. //__________________________________________________________________________
  78. MpdTpcKalmanTrack & MpdTpcKalmanTrack::operator=(const MpdTpcKalmanTrack& track)
  79. {
  80. /// Asignment operator
  81. // check assignement to self
  82. if (this == &track) return *this;
  83. // base class assignement
  84. MpdKalmanTrack::operator=(track);
  85. if (fTrHits) { fTrHits->Delete(); delete fTrHits; }
  86. fTrHits = new TClonesArray("MpdKalmanHit",70);
  87. Int_t nHits = track.fTrHits->GetEntriesFast();
  88. for (Int_t i = 0; i < nHits; ++i) {
  89. MpdKalmanHit *hit = (MpdKalmanHit*)(track.fTrHits->UncheckedAt(i));
  90. new ((*fTrHits)[i]) MpdKalmanHit(*hit);
  91. }
  92. if (track.fHits == 0x0) {
  93. for (Int_t i = 0; i < nHits; ++i) {
  94. fHits->Add(fTrHits->UncheckedAt(i));
  95. }
  96. }
  97. return *this;
  98. }
  99. //__________________________________________________________________________
  100. MpdTpcKalmanTrack::~MpdTpcKalmanTrack()
  101. {
  102. /// Destructor
  103. if (fTrHits) fTrHits->Delete();
  104. delete fTrHits;
  105. fTrHits = NULL;
  106. }
  107. //__________________________________________________________________________
  108. void MpdTpcKalmanTrack::Reset()
  109. {
  110. /// Reset track
  111. if (fTrHits) fTrHits->Delete();
  112. delete fTrHits;
  113. fTrHits = NULL;
  114. Clear();
  115. }
  116. //__________________________________________________________________________
  117. void MpdTpcKalmanTrack::EvalParams(MpdKalmanHit *hitOut, const MpdKalmanHit *hitIn,
  118. Double_t *parOut, Double_t *parIn, Double_t pt, Double_t *params)
  119. {
  120. /// Evaluate track params
  121. Double_t rIn = parIn[0], phiIn = parIn[1];
  122. Double_t rOut = parOut[0], phiOut = parOut[1];
  123. Double_t xIn = rIn * TMath::Cos(phiIn);
  124. Double_t yIn = rIn * TMath::Sin(phiIn);
  125. Double_t xOut = rOut * TMath::Cos(phiOut);
  126. Double_t yOut = rOut * TMath::Sin(phiOut);
  127. parOut[2] = xOut;
  128. parOut[3] = yOut;
  129. parIn[2] = xIn;
  130. parIn[3] = yIn;
  131. fPos = fPosNew = rOut;
  132. (*fParam)(0,0) = rOut * phiOut; // R-Phi coordinate
  133. //(*fParam)(1,0) = posOut.Z(); // Z-coordinate
  134. (*fParam)(1,0) = hitOut->GetMeas(1); // Z-coordinate
  135. Double_t phi0 = TMath::ATan2 (yOut-yIn, xOut-xIn); // Track Phi - approximation
  136. (*fParam)(2,0) = phi0 + TMath::Sign (params[2]/2, pt);
  137. //(*fParam)(3,0) = TMath::ATan2 (hitOut->GetMeas(1)-hitIn->GetMeas(1), rOut-rIn); // Track Theta
  138. (*fParam)(3,0) = TMath::ATan2 (hitOut->GetMeas(1)-hitIn->GetMeas(1), TMath::Abs(params[0])); // Track Theta
  139. (*fParam)(4,0) = 1. / pt; // q/Pt
  140. // Debugging
  141. /*
  142. TClonesArray *hits = (TClonesArray*) FairRootManager::Instance()->GetObject("TpcHit");
  143. TClonesArray *points = (TClonesArray*) FairRootManager::Instance()->GetObject("TpcPoint");
  144. MpdTpcHit *h = (MpdTpcHit*) hits->UncheckedAt(hitOut->GetIndex());
  145. TpcPoint *p = (TpcPoint*) points->UncheckedAt(h->GetRefIndex());
  146. cout << " Phi angle: " << TMath::ATan2(p->GetPy(),p->GetPx()) << " " << phi0 << " " << (*fParam)(2,0) << endl;
  147. */
  148. *fParamNew = *fParam;
  149. fWeight->Zero();
  150. EvalCovar(hitOut, hitIn, parOut, parIn);
  151. fHits->Add(hitOut);
  152. fLastLay = hitOut->GetLayer(); // layer number
  153. }
  154. //__________________________________________________________________________
  155. void MpdTpcKalmanTrack::EvalCovar(const MpdKalmanHit *hitOut, const MpdKalmanHit *hitIn,
  156. Double_t *parOut, Double_t *parIn)
  157. {
  158. /// Evaluate covariance matrix for track seed
  159. (*fWeight)(0,0) = hitOut->GetErr(0) * hitOut->GetErr(0); // <RphiRphi>
  160. (*fWeight)(0,0) *= 4.; // extra factor of 2
  161. (*fWeight)(1,1) = hitOut->GetErr(1) * hitOut->GetErr(1); // <zz>
  162. Double_t phiOut = parOut[1], phiIn = parIn[1];
  163. Double_t dx = parOut[2] - parIn[2], dy = parOut[3] - parIn[3];
  164. Double_t dist2 = dx * dx + dy * dy;
  165. Double_t sinPhi = TMath::Sin ((*fParam)(2,0));
  166. Double_t cosPhi = TMath::Cos ((*fParam)(2,0));
  167. Double_t pOut = TMath::Cos(phiOut) * cosPhi + TMath::Sin(phiOut) * sinPhi;
  168. Double_t pIn = TMath::Cos(phiIn) * cosPhi + TMath::Sin(phiIn) * sinPhi;
  169. (*fWeight)(2,2) = (pOut * pOut + pIn * pIn) / dist2 * (*fWeight)(0,0); // <PhiPhi>
  170. (*fWeight)(2,2) *= 4.; // extra factor of 2
  171. Double_t tanThe = TMath::Tan((*fParam)(3,0));
  172. //31.12.2018 Double_t dRad = parOut[0] - parIn[0];
  173. Double_t dRad = TMath::Sqrt (dist2);
  174. Double_t denom = dRad * (1.+tanThe*tanThe);
  175. (*fWeight)(3,3) = (*fWeight)(1,1) * 2. / denom / denom; // <TheThe>
  176. //(*fWeight)(3,3) = (*fWeight)(1,1) * 20. / denom / denom; // <TheThe>
  177. //(*fWeight)(4,4) = ((*fParam)(4,0)*0.5) * ((*fParam)(4,0)*0.5); // error 50%
  178. //(*fWeight)(4,4) = ((*fParam)(4,0)*0.75) * ((*fParam)(4,0)*0.75); // error 75%
  179. (*fWeight)(4,4) = ((*fParam)(4,0)*2.) * ((*fParam)(4,0)*2.); // error 200%
  180. //for (Int_t jj = 0; jj < 5; ++jj) cout << TMath::Sqrt((*fWeight)(jj,jj)) << " ";
  181. //cout << endl;
  182. //fWeight->Print();
  183. //fWeight->Invert(); // weight matrix
  184. Int_t iok = 0;
  185. MpdKalmanFilter::Instance()->MnvertLocal(fWeight->GetMatrixArray(), 5, 5, 5, iok);
  186. //fWeight->Print();
  187. }
  188. //__________________________________________________________________________
  189. void MpdTpcKalmanTrack::StartBack()
  190. {
  191. /// Prepare for back tracing
  192. //#pragma omp critical
  193. {
  194. fHits->Sort(); // in descending order in radius
  195. }
  196. fChi2 = 0.;
  197. //fTrackDir = kInward;
  198. //if (fLastLay == 50) { cout << " back" << endl; Weight2Cov()->Print(); exit(0); }
  199. Int_t nHits = GetNofHits();
  200. if (nHits == 1) return;
  201. nHits *= nHits;
  202. //*
  203. for (Int_t i = 0; i < 5; ++i) {
  204. for (Int_t j = i; j < 5; ++j) {
  205. //if (j == i) (*fWeight)(i,j) /= 100.;
  206. if (j == i) (*fWeight)(i,j) /= nHits;
  207. else (*fWeight)(i,j) = (*fWeight)(j,i) = 0.;
  208. }
  209. }
  210. //*/
  211. //fWeight->Zero();
  212. //EvalCovar((MpdKalmanHitR*)fHits->UncheckedAt(0),(MpdKalmanHitR*)fHits->UncheckedAt(1));
  213. }
  214. //__________________________________________________________________________
  215. Int_t MpdTpcKalmanTrack::Compare(const TObject* track) const
  216. {
  217. /// "Compare" function to sort in descending order in pt
  218. MpdKalmanTrack *trackKF = (MpdKalmanTrack*) track;
  219. Double_t pt = 1. / TMath::Abs(trackKF->GetParam(4));
  220. Double_t ptthis = 1. / TMath::Abs((*fParam)(4,0));
  221. if (ptthis < pt) return 1;
  222. else if (ptthis > pt) return -1;
  223. return 0;
  224. }
  225. //__________________________________________________________________________
  226. Bool_t MpdTpcKalmanTrack::GetRecoQuality(Double_t dist, Double_t percentage)
  227. {
  228. /// returns kTRUE if number of hits closer to boundaries than dist divided by nHits is larger than percentage
  229. MpdKalmanHit *hit = NULL;
  230. Int_t nCloseHits = 0;
  231. Int_t nTrHits = fTrHits->GetEntries();
  232. if (nTrHits == 0) {cout << "NO KALMAN HITS ARE FOUND" << endl; return kTRUE;}
  233. for (Int_t itr = 0; itr < nTrHits; itr++)
  234. {
  235. hit = (MpdKalmanHit*) fTrHits->UncheckedAt(itr);
  236. if (TMath::Abs(hit->GetEdge()) < dist) nCloseHits++;
  237. }
  238. if ((Double_t)(nCloseHits/(Double_t)nTrHits) > percentage) return kTRUE;
  239. else return kFALSE;
  240. }
  241. ClassImp(MpdTpcKalmanTrack)