Newer
Older
TB_Chris / TbEvent / src / .svn / text-base / TbKalmanTrack.cpp.svn-base
  1.  
  2. #include "Event/TbKalmanTrack.h"
  3. #include "Event/TbKalmanPixelMeasurement.h"
  4.  
  5. namespace {
  6.  
  7. struct OrderInZ {
  8. bool operator()(const LHCb::TbKalmanNode* lhs,
  9. const LHCb::TbKalmanNode* rhs) const {
  10. return lhs->z() < rhs->z();
  11. }
  12. };
  13.  
  14. double errorfromcov(double cov) { return cov > 0 ? std::sqrt(cov) : cov; }
  15.  
  16. void printState(const LHCb::TbState& state, std::ostream& os) {
  17. os << "(" << state.parameters()(0) << "," << state.parameters()(1) << ","
  18. << state.parameters()(2) << "," << state.parameters()(3) << ") +/- ("
  19. << errorfromcov(state.covariance()(0, 0)) << ","
  20. << errorfromcov(state.covariance()(1, 1)) << ","
  21. << errorfromcov(state.covariance()(2, 2)) << ","
  22. << errorfromcov(state.covariance()(3, 3)) << ")";
  23. }
  24. }
  25.  
  26. namespace LHCb {
  27. //============================================================================
  28. // Constructor
  29. //============================================================================
  30. TbKalmanTrack::TbKalmanTrack(const LHCb::TbTrack& track, const double hiterror2,
  31. const double noise2)
  32. : TbTrack(track) {
  33. // Create nodes for all clusters
  34. for (const LHCb::TbCluster* cluster : track.clusters()) {
  35. TbKalmanNode* node = new TbKalmanPixelMeasurement(*this, *cluster, hiterror2);
  36. node->setNoise2(noise2);
  37. m_nodes.push_back(node);
  38. }
  39.  
  40. // Make sure they are sorted in z
  41. std::sort(m_nodes.begin(), m_nodes.end(), OrderInZ());
  42.  
  43. // Now set the links
  44. TbKalmanNode* prevnode(nullptr);
  45. for (LHCb::TbKalmanNode* node : m_nodes) {
  46. node->link(prevnode);
  47. prevnode = node;
  48. }
  49. }
  50.  
  51. //============================================================================
  52. // Destructor
  53. //============================================================================
  54. TbKalmanTrack::~TbKalmanTrack() {
  55. for (LHCb::TbKalmanNode* node : m_nodes) delete node;
  56. }
  57.  
  58. //============================================================================
  59. // Add a node
  60. //============================================================================
  61. void TbKalmanTrack::addNode(TbKalmanNode* node) {
  62. // This can be optimized a little bit by insorting in the right place.
  63. // If so, make sure to fix the 'link' method in TbKalmanNode
  64. m_nodes.push_back(node);
  65. // Make sure the nodes are sorted in z
  66. std::sort(m_nodes.begin(), m_nodes.end(), OrderInZ());
  67. // Now set the links
  68. TbKalmanNode* prevnode(nullptr);
  69. for (LHCb::TbKalmanNode* node : m_nodes) {
  70. node->link(prevnode);
  71. prevnode = node;
  72. }
  73. }
  74.  
  75. //============================================================================
  76. // Add a reference node
  77. //============================================================================
  78. void TbKalmanTrack::addReferenceNode(double z) {
  79. addNode(new TbKalmanNode(*this, z));
  80. }
  81.  
  82. //============================================================================
  83. // Deactivate a measurement
  84. //============================================================================
  85. void TbKalmanTrack::deactivateCluster(const TbCluster& clus) {
  86. for (LHCb::TbKalmanNode* node : m_nodes) {
  87. TbKalmanPixelMeasurement* meas =
  88. dynamic_cast<TbKalmanPixelMeasurement*>(node);
  89. if (meas && &(meas->cluster()) == &clus)
  90. meas->deactivateMeasurement(true);
  91. }
  92. }
  93.  
  94. //============================================================================
  95. // Perform the fit
  96. //============================================================================
  97. void TbKalmanTrack::fit() {
  98. // remove existing states
  99. clearStates();
  100.  
  101. // do the fit
  102. if (!m_nodes.empty()) {
  103. // initialize the seed: very simple for now. later we may want
  104. // to run some iterations and then this becomes more
  105. // complicated.
  106. LHCb::TbState seedstate(firstState());
  107. Gaudi::SymMatrix4x4 seedcov;
  108. seedcov(0, 0) = seedcov(1, 1) = 1e4;
  109. seedcov(2, 2) = seedcov(3, 3) = 1;
  110. seedstate.covariance() = seedcov;
  111. m_nodes.front()->setSeed(seedstate);
  112. m_nodes.back()->setSeed(seedstate);
  113.  
  114. // everything happens on demand, I hope. so all we need to do is copy the
  115. // smoothed states back.
  116. LHCb::ChiSquare chi2(0, -4);
  117. for (LHCb::TbKalmanNode* node : m_nodes) {
  118. // get the smoothed state
  119. addToStates(node->state());
  120. // add to the chi2
  121. chi2 += node->deltaChi2(0);
  122. }
  123.  
  124. setNdof(chi2.nDoF());
  125. if (chi2.nDoF() > 0) {
  126. setChi2PerNdof(chi2.chi2() / chi2.nDoF());
  127. } else {
  128. setChi2PerNdof(0);
  129. }
  130. }
  131. }
  132.  
  133. //============================================================================
  134. // Print some debug info
  135. //============================================================================
  136. void TbKalmanTrack::print() const {
  137. std::cout << "This is a kalman fitted tracks with chi2/ndof=" << chi2() << "/"
  138. << ndof() << std::endl;
  139. // compute forward/backward chi2
  140. LHCb::ChiSquare forwardchi2, backwardchi2;
  141. double chi2X(0), chi2Y(0);
  142. std::cout << "These are the nodes, with some residuals: " << std::endl;
  143. for (const LHCb::TbKalmanNode * node : m_nodes) {
  144. std::cout << node->index() << " " << node->z() << " ";
  145. //<< node->hasInfoUpstream( LHCb::TbKalmanNode::Forward) << " "
  146. //<< node->hasInfoUpstream( LHCb::TbKalmanNode::Backward) << " " ;
  147. printState(node->state(), std::cout);
  148. //std::cout << node->filterStatus( LHCb::TbKalmanNode::Forward) << " "
  149. //<< node->filterStatus( LHCb::TbKalmanNode::Backward) << " " ;
  150. const TbKalmanPixelMeasurement* pixelhit =
  151. dynamic_cast<const TbKalmanPixelMeasurement*>(node);
  152. if (pixelhit) {
  153. std::cout << "residual x = " << pixelhit->residualX() << " +/- "
  154. << std::sqrt(pixelhit->residualCovX()) << " "
  155. << "residual y = " << pixelhit->residualY() << " +/- "
  156. << std::sqrt(pixelhit->residualCovY()) << " ";
  157. chi2X += pixelhit->residualX() * pixelhit->residualX() / pixelhit->covX();
  158. chi2Y += pixelhit->residualY() * pixelhit->residualY() / pixelhit->covY();
  159. }
  160. std::cout << std::endl;
  161. forwardchi2 += node->deltaChi2(LHCb::TbKalmanNode::Forward);
  162. backwardchi2 += node->deltaChi2(LHCb::TbKalmanNode::Backward);
  163. }
  164. std::cout << "Forward/backward chi2: " << forwardchi2.chi2() << "/"
  165. << backwardchi2.chi2() << std::endl;
  166. std::cout << "X/Y chi2: " << chi2X << "/" << chi2Y << std::endl;
  167. }
  168. }