Newer
Older
TB_Chris / TbKernel / src / .svn / text-base / TbTrackFit.cpp.svn-base
  1. // Tb/TbEvent
  2. #include "Event/TbCluster.h"
  3. #include "Event/TbTrack.h"
  4.  
  5. // Local
  6. #include "TbTrackFit.h"
  7.  
  8. DECLARE_TOOL_FACTORY(TbTrackFit)
  9.  
  10. //=============================================================================
  11. // Standard constructor, initializes variables
  12. //=============================================================================
  13. TbTrackFit::TbTrackFit(const std::string& type, const std::string& name,
  14. const IInterface* parent)
  15. : GaudiTool(type, name, parent), m_geomSvc(nullptr) {
  16.  
  17. declareInterface<ITbTrackFit>(this);
  18.  
  19. declareProperty("MaskedPlanes", m_maskedPlanes = {});
  20. }
  21.  
  22. //=============================================================================
  23. // Destructor
  24. //=============================================================================
  25. TbTrackFit::~TbTrackFit() {}
  26.  
  27. //=============================================================================
  28. // Initialisation
  29. //=============================================================================
  30. StatusCode TbTrackFit::initialize() {
  31.  
  32. // Initialise the base class.
  33. StatusCode sc = GaudiTool::initialize();
  34. if (sc.isFailure()) return sc;
  35. // Get the number of telescope planes.
  36. const auto nPlanes = geomSvc()->modules().size();
  37. // Set the flags whether a plane is masked or not.
  38. m_masked.resize(nPlanes, false);
  39. for (const unsigned int plane : m_maskedPlanes) {
  40. m_masked[plane] = true;
  41. }
  42. return StatusCode::SUCCESS;
  43. }
  44.  
  45. //=============================================================================
  46. // Exclude a plane from the fit
  47. //=============================================================================
  48. void TbTrackFit::maskPlane(const unsigned int plane) {
  49.  
  50. m_masked[plane] = true;
  51. }
  52.  
  53. //=============================================================================
  54. // (Re-)include a plane in the fit
  55. //=============================================================================
  56. void TbTrackFit::unmaskPlane(const unsigned int plane) {
  57.  
  58. m_masked[plane] = false;
  59. }
  60.  
  61. //=========================================================================
  62. /// Perform a straight-line fit to the clusters of a given track
  63. //=========================================================================
  64. void TbTrackFit::fit(LHCb::TbTrack* track) {
  65.  
  66. // Sums for the x-slope fit
  67. double s0 = 0.;
  68. double sx = 0.;
  69. double sz = 0.;
  70. double sxz = 0.;
  71. double sz2 = 0.;
  72. // Sums for the y-slope fit
  73. double u0 = 0.;
  74. double uy = 0.;
  75. double uz = 0.;
  76. double uyz = 0.;
  77. double uz2 = 0.;
  78.  
  79. // Count the number of planes included in the fit.
  80. unsigned int nd = 0;
  81.  
  82. // Loop through the clusters.
  83. auto clusters = track->clusters();
  84. for (auto it = clusters.cbegin(), end = clusters.cend(); it != end; ++it) {
  85. if (!(*it)) continue;
  86. // Skip masked planes.
  87. if (m_masked[(*it)->plane()]) continue;
  88. ++nd;
  89. const double wx = (*it)->wx();
  90. const double wy = (*it)->wy();
  91. const double x = (*it)->x();
  92. const double y = (*it)->y();
  93. const double z = (*it)->z();
  94.  
  95. // Straight line fit in x
  96. s0 += wx;
  97. sx += wx * x;
  98. sz += wx * z;
  99. sxz += wx * x * z;
  100. sz2 += wx * z * z;
  101. // Straight line fit in y
  102. u0 += wy;
  103. uy += wy * y;
  104. uz += wy * z;
  105. uyz += wy * y * z;
  106. uz2 += wy * z * z;
  107. }
  108. // if (nd < 3) {
  109. // error() << "Invalid track. Only " << nd << " non-masked clusters" <<
  110. // endmsg;
  111. // return;
  112. // }
  113. // Compute the track parameters for x.
  114.  
  115. double den = (sz2 * s0 - sz * sz);
  116.  
  117. if (fabs(den) < 10e-10) den = 1.;
  118. const double tx = (sxz * s0 - sx * sz) / den;
  119. const double x0 = (sx * sz2 - sxz * sz) / den;
  120.  
  121. // Compute the track parameters for y.
  122. den = (uz2 * u0 - uz * uz);
  123. if (fabs(den) < 10e-10) den = 1.;
  124. const double ty = (uyz * u0 - uy * uz) / den;
  125. const double y0 = (uy * uz2 - uyz * uz) / den;
  126.  
  127. // Calculate the covariance matrix.
  128. // Ad hoc matrix inversion, as it is almost diagonal.
  129. // Some terms vanish since we calculate the covariance matrix at z = 0.
  130. const double m00 = s0;
  131. const double m11 = u0;
  132. const double m20 = sz;
  133. const double m31 = uz;
  134. const double m22 = sz2;
  135. const double m33 = uz2;
  136. const double den20 = m22 * m00 - m20 * m20;
  137. const double den31 = m33 * m11 - m31 * m31;
  138.  
  139. Gaudi::SymMatrix4x4 cov;
  140. cov(0, 0) = m22 / den20;
  141. cov(2, 0) = -m20 / den20;
  142. cov(2, 2) = m00 / den20;
  143.  
  144. cov(1, 1) = m33 / den31;
  145. cov(3, 1) = -m31 / den31;
  146. cov(3, 3) = m11 / den31;
  147.  
  148. // Create the first state.
  149. LHCb::TbState fstate(Gaudi::Vector4(x0, y0, tx, ty), cov, 0., 0);
  150.  
  151. fstate.covariance()(0, 0) = cov(0, 0);
  152. fstate.covariance()(0, 2) = cov(2, 0);
  153. fstate.covariance()(1, 1) = cov(1, 1);
  154. fstate.covariance()(2, 2) = cov(2, 2);
  155. fstate.covariance()(1, 3) = cov(3, 1);
  156. fstate.covariance()(3, 3) = cov(3, 3);
  157. // Update the firstState of the track.
  158. track->setFirstState(fstate);
  159.  
  160. // Compute chi2 and track time.
  161. double chi2 = 0.;
  162. double time = 0.;
  163. for (auto it = clusters.cbegin(), end = clusters.cend(); it != end; ++it) {
  164. if (!(*it)) continue;
  165. // Skip masked planes.
  166. if (m_masked[(*it)->plane()]) continue;
  167. const double wx = (*it)->wx();
  168. const double wy = (*it)->wy();
  169. const unsigned int plane = (*it)->plane();
  170. // Calculate global (biased) residuals in x and y.
  171. const Gaudi::XYZPoint intercept = geomSvc()->intercept(track, plane);
  172. const Gaudi::XYZPoint local = geomSvc()->globalToLocal(intercept, plane);
  173.  
  174. const double dx = (*it)->xloc() - local.x();
  175. const double dy = (*it)->yloc() - local.y();
  176. chi2 += (dx * dx) * wx + (dy * dy) * wy;
  177. time += (*it)->htime();
  178. }
  179. // Set track chi2PerNdof and ndof.
  180. const unsigned int ndof = 2 * nd - 4;
  181. track->setNdof(ndof);
  182. track->setChi2PerNdof(chi2 / (double)ndof);
  183.  
  184. // Finally, set the track time.
  185. time /= nd;
  186. track->setHtime(time);
  187. }