Belle II Software  release-05-02-19
DistanceCalculatorModule.cc
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * Copyright(C) 2019 - Belle II Collaboration *
4  * *
5  * Author: The Belle II Collaboration *
6  * Contributors: Sourav Dey, Abi Soffer *
7  * Past Contributors: Omer Benami *
8  * This software is provided "as is" without any warranty. *
9  **************************************************************************/
10 
11 #include <analysis/modules/DistanceCalculator/DistanceCalculatorModule.h>
12 #include <analysis/dataobjects/Particle.h>
13 #include <analysis/dataobjects/ParticleList.h>
14 #include <analysis/dataobjects/Btube.h>
15 
16 #include <TMath.h>
17 #include <TVectorF.h>
18 #include <TMatrixFSym.h>
19 #include <iostream>
20 
21 #include <Eigen/Core>
22 #include <Eigen/Geometry>
23 
24 using namespace std;
25 using namespace Belle2;
26 
27 const float eps = 1E-6;
28 
29 //-----------------------------------------------------------------
30 // Register the Module
31 //-----------------------------------------------------------------
32 REG_MODULE(DistanceCalculator)
33 
34 //-----------------------------------------------------------------
35 // Implementation
36 //-----------------------------------------------------------------
37 
39 {
40  // Set module properties
41  setDescription("Calculates distance between two vertices, distance of closest approach between a vertex and a track, distance of closest approach between two tracks, distance of closest approach between a vertex/track and Btube");
42 
43  // Parameter definitions
44  addParam("listName", m_listName, "", std::string(""));
45  addParam("decayString", m_decayString, "", std::string(""));
46  addParam("mode", m_mode,
47  "Specifies how the distance is calculated:\n"
48  "vertextrack: calculate the distance of closest approach between a track and a vertex, taking the first candidate as vertex, default\n"
49  "trackvertex: calculate the distance of closest approach between a track and a vertex, taking the first candidate as track,\n"
50  "2tracks: calculates the distance of closest approach between two tracks,\n"
51  "2vertices: calculates the distance between two vertices,\n"
52  "vertexbtube: calculates the distance of closest approach between a vertex and a Btube,\n"
53  "trackbtube: calculates the distance of closest approach between a track and a Btube",
54  std::string("vertextrack"));
55 }
56 
57 DistanceCalculatorModule::~DistanceCalculatorModule()
58 {
59 }
60 
61 void DistanceCalculatorModule::initialize()
62 {
63  if (m_decayString != "") {
64  m_decayDescriptor.init(m_decayString);
65  } else {
66  B2FATAL("Please provide a decay string for DistanceCalculatorModule");
67  }
68 }
69 
70 Eigen::Vector3d getDocaTrackVertex(const Particle* pTrack, const Particle* pVertex)
71 {
72  Eigen::Vector3d v(pTrack->getPx(), pTrack->getPy(), pTrack->getPz());
73  Eigen::Vector3d p_ray(pTrack->getX(), pTrack->getY(), pTrack->getZ());
74  Eigen::Vector3d p(pVertex->getX(), pVertex->getY(), pVertex->getZ());
75  Eigen::Vector3d v_dir = v.normalized();
76  Eigen::Vector3d r = p - p_ray;
77  return r - r.dot(v_dir) * v_dir;
78 }
79 
80 TMatrixFSym getDocaTrackVertexError(const Particle* pTrack, const Particle* pVertex)
81 {
82  //covariance matrix of r
83  TMatrixFSym err_r = pTrack->getVertexErrorMatrix() + pVertex->getVertexErrorMatrix();
84  Eigen::Vector3d V(pTrack->getPx(), pTrack->getPy(), pTrack->getPz());
85  Eigen::Vector3d v = V.normalized();
86  //d_j = r_j - v_j * v_k r_k
87  //Jij = del_i d_j = delta_ij - v_i * v_j
88  //Since the vector of closest approach is a linear function of r, its
89  //propagation of errors is exact
90  TMatrixFSym Jacobian(3);
91  //Fill the jacobian matrix according to the equation:
92  // J_ij = delta_ij -v(i)v(j)
93  for (int i = 0; i < 3; i++)
94  for (int j = 0; j < 3; j++)
95  Jacobian(i, j) = -v(i) * v(j);
96  for (int i = 0; i < 3; i++)
97  Jacobian(i, i) += 1;
98 
99  return err_r.Similarity(Jacobian); //calculates J * err_r * J^T, and returns it
100 }
101 
102 /*
103  * This method calculates the Doca vector between two tracks (i.e. lines)
104  * This is done using the following equation:
105  * d = (n*(x1-x2))n
106  * where d is the doca vector, n is a unit vector that is orthogonal to both lines,
107  * and x1 and x2 are points on the first and second line respectively
108  */
109 Eigen::Vector3d getDocaTracks(const Particle* p1, const Particle* p2)
110 {
111  Eigen::Vector3d v1(p1->getPx(), p1->getPy(), p1->getPz());
112  Eigen::Vector3d v2(p2->getPx(), p2->getPy(), p2->getPz());
113  Eigen::Vector3d n = v1.cross(v2); //The doca vector must be orthogonal to both lines.
114  if (n.norm() < eps) {
115  //if tracks are parallel then the distance is independant of the point on the line
116  //and we can use the old way to calculate the DOCA
117  return getDocaTrackVertex(p1, p2);
118  }
119  Eigen::Vector3d n_dir = n.normalized();
120  Eigen::Vector3d p1v(p1->getX(), p1->getY(), p1->getZ());
121  Eigen::Vector3d p2v(p2->getX(), p2->getY(), p2->getZ());
122  Eigen::Vector3d r = p1v - p2v;
123  return r.dot(n_dir) * n_dir;
124 }
125 
126 TMatrixFSym getDocaTracksError(const Particle* p1, const Particle* p2)
127 {
128  //covariance matrix of r
129  TMatrixFSym err_r = p1->getVertexErrorMatrix() + p2->getVertexErrorMatrix();
130  Eigen::Vector3d p1p(p1->getPx(), p1->getPy(), p1->getPz());
131  Eigen::Vector3d p2p(p2->getPx(), p2->getPy(), p2->getPz());
132  Eigen::Vector3d n = p1p.cross(p2p);
133  if (n.norm() < eps) {
134  return getDocaTrackVertexError(p1, p2);
135  }
136  Eigen::Vector3d n_dir = n.normalized();
137  //d_j = n_dir_j * n_dir_k r_k
138  //Jij = del_i d_j = n_dir_i * n_dir_j
139  //Since the vector of closest approch is a linear function of r, it's
140  //propagation of errors is exact
141  TMatrixFSym Jacobian(3);
142  //Fill the jacobian matrix accodring to the equation:
143  // J_ij = n(i)n(j)
144  for (int i = 0; i < 3; i++)
145  for (int j = 0; j < 3; j++)
146  Jacobian(i, j) = n_dir(i) * n_dir(j);
147  return err_r.Similarity(Jacobian); //calculates J * err_r * J^T, and returns it
148 }
149 
150 Eigen::Vector3d getDistanceVertices(const Particle* p1, const Particle* p2)
151 {
152  Eigen::Vector3d p1v(p1->getX(), p1->getY(), p1->getZ());
153  Eigen::Vector3d p2v(p2->getX(), p2->getY(), p2->getZ());
154  Eigen::Vector3d r = p1v - p2v;
155  return r;
156 }
157 TMatrixFSym getDistanceVerticesErrors(const Particle* p1, const Particle* p2)
158 {
159  TMatrixFSym err_r = p1->getVertexErrorMatrix() + p2->getVertexErrorMatrix();
160  return err_r;
161 }
162 
163 Eigen::Vector3d getDistance(const Particle* p1, const Particle* p2, const std::string& mode)
164 {
165  if (mode == "2tracks") {
166  return getDocaTracks(p1, p2);
167  }
168  if (mode == "2vertices") {
169  return getDistanceVertices(p1, p2);
170  }
171  if (mode == "trackvertex") {
172  return getDocaTrackVertex(p2, p1);
173  }
174  //if(mode == "vertextrack")
175  return getDocaTrackVertex(p1, p2);
176 }
177 TMatrixFSym getDistanceErrors(const Particle* p1, const Particle* p2, const std::string& mode)
178 {
179  if (mode == "2tracks") {
180  return getDocaTracksError(p1, p2);
181  }
182  if (mode == "2vertices") {
183  return getDistanceVerticesErrors(p1, p2);
184  }
185  if (mode == "trackvertex") {
186  return getDocaTrackVertexError(p2, p1);
187  }
188  // if(mode == "vertextrack"){
189  return getDocaTrackVertexError(p1, p2);
190 }
191 
192 Eigen::Vector3d getDocaBtubeVertex(const Particle* pVertex, const Btube* tube)
193 {
194  Eigen::Vector3d v(tube->getTubeDirection()(0, 0), tube->getTubeDirection()(1, 0), tube->getTubeDirection()(2, 0));
195  Eigen::Vector3d p_ray(tube->getTubeCenter()(0, 0), tube->getTubeCenter()(1, 0), tube->getTubeCenter()(2, 0));
196  Eigen::Vector3d p(pVertex->getX(), pVertex->getY(), pVertex->getZ());
197  Eigen::Vector3d v_dir = v.normalized();
198  Eigen::Vector3d r = p - p_ray;
199  return r - r.dot(v_dir) * v_dir;
200 }
201 
202 TMatrixFSym getDocaBtubeVertexError(const Particle* pVertex, const Btube* tube)
203 {
204  TMatrixFSym err_r = tube->getTubeCenterErrorMatrix() + pVertex->getVertexErrorMatrix();
205  Eigen::Vector3d V(tube->getTubeDirection()(0, 0), tube->getTubeDirection()(1, 0), tube->getTubeDirection()(2, 0));
206  Eigen::Vector3d v = V.normalized();
207  TMatrixFSym Jacobian(3);
208  for (int i = 0; i < 3; i++)
209  for (int j = 0; j < 3; j++)
210  Jacobian(i, j) = -v(i) * v(j);
211  for (int i = 0; i < 3; i++)
212  Jacobian(i, i) += 1;
213 
214  return err_r.Similarity(Jacobian);
215 }
216 
217 Eigen::Vector3d getDocaBtubeTrack(const Particle* pTrack, const Btube* tube)
218 {
219  Eigen::Vector3d v1(pTrack->getPx(), pTrack->getPy(), pTrack->getPz());
220  Eigen::Vector3d v2 = tube->getTubeDirection();
221  Eigen::Vector3d n = v1.cross(v2); //The doca vector must be orthogonal to both lines.
222  if (n.norm() < eps) {
223  //if tracks are parallel then the distance is independant of the point on the line
224  //and we can use the old way to calculate the DOCA
225  return getDocaBtubeVertex(pTrack, tube);
226  }
227  Eigen::Vector3d n_dir = n.normalized();
228  Eigen::Vector3d pv(pTrack->getX(), pTrack->getY(), pTrack->getZ());
229  Eigen::Vector3d p2v(tube->getTubeCenter()(0, 0), tube->getTubeCenter()(1, 0), tube->getTubeCenter()(2, 0));
230  Eigen::Vector3d r = pv - p2v;
231  return r.dot(n_dir) * n_dir;
232 }
233 
234 TMatrixFSym getDocaBtubeTrackError(const Particle* pTrack, const Btube* tube)
235 {
236  TMatrixFSym err_r = pTrack->getVertexErrorMatrix() + tube->getTubeCenterErrorMatrix();
237  Eigen::Vector3d p1p(pTrack->getPx(), pTrack->getPy(), pTrack->getPz());
238  Eigen::Vector3d p2p(tube->getTubeDirection()(0, 0), tube->getTubeDirection()(1, 0), tube->getTubeDirection()(2, 0));
239  Eigen::Vector3d n = p1p.cross(p2p);
240  if (n.norm() < eps) {
241  return getDocaBtubeVertexError(pTrack, tube);
242  }
243  Eigen::Vector3d n_dir = n.normalized();
244  TMatrixFSym Jacobian(3);
245  for (int i = 0; i < 3; i++)
246  for (int j = 0; j < 3; j++)
247  Jacobian(i, j) = n_dir(i) * n_dir(j);
248  return err_r.Similarity(Jacobian);
249 }
250 Eigen::Vector3d getBtubeDistance(const Particle* p, const Btube* t, const std::string& mode)
251 {
252  if (mode == "vertexbtube") {
253  return getDocaBtubeVertex(p, t);
254  }
255  // if (mode == "trackbtube") {
256  return getDocaBtubeTrack(p, t);
257  //}
258 }
259 TMatrixFSym getBtubeDistanceErrors(const Particle* p, const Btube* t, const std::string& mode)
260 {
261  if (mode == "vertexbtube") {
262  return getDocaBtubeVertexError(p, t);
263  }
264  //if (mode == "trackbtube") {
265  return getDocaBtubeTrackError(p, t);
266  //}
267 }
268 
269 void DistanceCalculatorModule::event()
270 {
271  StoreObjPtr<ParticleList> plist(m_listName);
272  unsigned int n = plist->getListSize();
273  for (unsigned int i = 0; i < n; i++) {
274  Particle* particle = plist->getParticle(i);
275  std::vector<const Particle*> selectedParticles = m_decayDescriptor.getSelectionParticles(particle);
276  Eigen::Vector3d Distance;
277  TMatrixFSym DistanceCovMatrix(3);
278  if ((m_mode == "vertexbtube") || (m_mode == "trackbtube")) {
279  if (selectedParticles.size() == 1) {
280  const Btube* t = particle->getRelatedTo<Btube>();
281  if (!t) {
282  B2FATAL("No associated Btube found");
283  }
284  const Particle* p0 = selectedParticles[0];
285  Distance = getBtubeDistance(p0, t, m_mode);
286  DistanceCovMatrix = getBtubeDistanceErrors(particle, t, m_mode);
287  }
288  } else {
289  if (selectedParticles.size() == 2) {
290  const Particle* p1 = selectedParticles[0];
291  const Particle* p2 = selectedParticles[1];
292  Distance = getDistance(p1, p2, m_mode);
293  DistanceCovMatrix = getDistanceErrors(p1, p2, m_mode);
294  }
295  }
296  Eigen::Matrix3d DistanceCovMatrix_eigen;
297  DistanceCovMatrix_eigen.resize(3, 3);
298  for (int row = 0; row < 3; row++) {
299  for (int column = 0; column < 3; column++) {
300  DistanceCovMatrix_eigen(row, column) = DistanceCovMatrix[row][column]; //converting from TMatrixFSym to Eigen
301  }
302  }
303  float DistanceMag = Distance.norm();
304  double dist_err = 0;
305  if (DistanceMag != 0) {
306  Eigen::Vector3d dist_dir = Distance.normalized();
307  dist_err = TMath::Sqrt(((dist_dir.transpose()) * (DistanceCovMatrix_eigen * dist_dir)).norm());
308  }
309  particle->writeExtraInfo("CalculatedDistance", DistanceMag);
310  particle->writeExtraInfo("CalculatedDistanceError", dist_err);
311  particle->writeExtraInfo("CalculatedDistanceVector_X", Distance(0));
312  particle->writeExtraInfo("CalculatedDistanceVector_Y", Distance(1));
313  particle->writeExtraInfo("CalculatedDistanceVector_Z", Distance(2));
314  //Save the Covariance Matrix
315  particle->writeExtraInfo("CalculatedDistanceCovMatrixXX", DistanceCovMatrix[0][0]);
316  particle->writeExtraInfo("CalculatedDistanceCovMatrixXY", DistanceCovMatrix[0][1]);
317  particle->writeExtraInfo("CalculatedDistanceCovMatrixXZ", DistanceCovMatrix[0][2]);
318  particle->writeExtraInfo("CalculatedDistanceCovMatrixYX", DistanceCovMatrix[1][0]);
319  particle->writeExtraInfo("CalculatedDistanceCovMatrixYY", DistanceCovMatrix[1][1]);
320  particle->writeExtraInfo("CalculatedDistanceCovMatrixYZ", DistanceCovMatrix[1][2]);
321  particle->writeExtraInfo("CalculatedDistanceCovMatrixZX", DistanceCovMatrix[2][0]);
322  particle->writeExtraInfo("CalculatedDistanceCovMatrixZY", DistanceCovMatrix[2][1]);
323  particle->writeExtraInfo("CalculatedDistanceCovMatrixZZ", DistanceCovMatrix[2][2]);
324  }
325 }
Belle2::Particle::getPx
float getPx() const
Returns x component of momentum.
Definition: Particle.h:502
VXDTFFilterTest::v2
const std::vector< double > v2
MATLAB generated random vector.
Definition: decorrelationMatrix.cc:44
Belle2::Particle::getZ
float getZ() const
Returns z component of vertex position.
Definition: Particle.h:556
Belle2::Particle::getPy
float getPy() const
Returns y component of momentum.
Definition: Particle.h:511
Belle2::DistanceCalculatorModule
Calculates distance between two vertices, distance of closest approach between a vertex and a track,...
Definition: DistanceCalculatorModule.h:39
Belle2::Particle::getPz
float getPz() const
Returns z component of momentum.
Definition: Particle.h:520
REG_MODULE
#define REG_MODULE(moduleName)
Register the given module (without 'Module' suffix) with the framework.
Definition: Module.h:652
Belle2::Particle::getY
float getY() const
Returns y component of vertex position.
Definition: Particle.h:547
VXDTFFilterTest::v1
const std::vector< double > v1
MATLAB generated random vector.
Definition: decorrelationMatrix.cc:30
Belle2::Btube
For each MCParticle with hits in the CDC, this class stores some summarising information on those hit...
Definition: Btube.h:38
Belle2::Module
Base class for Modules.
Definition: Module.h:74
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19
Belle2::StoreObjPtr
Type-safe access to single objects in the data store.
Definition: ParticleList.h:33
Belle2::Btube::getTubeDirection
Eigen::Matrix< double, 3, 1 > getTubeDirection() const
Returns Btube direction.
Definition: Btube.h:82
Belle2::Btube::getTubeCenter
Eigen::Matrix< double, 3, 1 > getTubeCenter() const
Returns Btube center.
Definition: Btube.h:75
Belle2::Particle::getX
float getX() const
Returns x component of vertex position.
Definition: Particle.h:538
Belle2::Particle
Class to store reconstructed particles.
Definition: Particle.h:77
Belle2::Particle::getVertexErrorMatrix
TMatrixFSym getVertexErrorMatrix() const
Returns the 3x3 position error sub-matrix.
Definition: Particle.cc:413
Belle2::Btube::getTubeCenterErrorMatrix
TMatrixFSym getTubeCenterErrorMatrix() const
Returns Btube Center Error Matrix.
Definition: Btube.h:96