Belle II Software  release-05-01-25
KFitBase.cc
1 /**************************************************************************
2  * Copyright(C) 1997 - J. Tanaka *
3  * *
4  * Author: J. Tanaka *
5  * Contributor: J. Tanaka and *
6  * conversion to Belle II structure by T. Higuchi *
7  * *
8  * This software is provided "as is" without any warranty. *
9  **************************************************************************/
10 
11 #include <TMatrixFSym.h>
12 
13 #include <analysis/utility/ROOTToCLHEP.h>
14 #include <analysis/VertexFitting/KFit/KFitBase.h>
15 
16 using namespace std;
17 using namespace Belle2;
18 using namespace Belle2::analysis;
19 using namespace CLHEP;
20 
21 KFitBase::KFitBase()
22 {
23  m_ErrorCode = KFitError::kNoError;
24  m_FlagFitted = false;
25  m_FlagCorrelation = false;
26  m_FlagOverIteration = false;
27  m_MagneticField = KFitConst::kDefaultMagneticField;
28  m_NDF = 0;
29  m_CHIsq = -1;
30  m_NecessaryTrackCount = -1;
31  m_TrackCount = 0;
32 }
33 
34 
35 KFitBase::~KFitBase() = default;
36 
37 
38 enum KFitError::ECode
39 KFitBase::addTrack(const KFitTrack& p) {
40  m_Tracks.push_back(p);
41  m_TrackCount = m_Tracks.size();
42 
43  return m_ErrorCode = KFitError::kNoError;
44 }
45 
46 
47 enum KFitError::ECode
48 KFitBase::addTrack(const HepLorentzVector& p, const HepPoint3D& x, const HepSymMatrix& e, const double q) {
49  if (e.num_row() != KFitConst::kNumber7)
50  {
51  m_ErrorCode = KFitError::kBadMatrixSize;
52  KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
53  return m_ErrorCode;
54  }
55 
56  return this->addTrack(KFitTrack(p, x, e, q));
57 }
58 
59 
60 enum KFitError::ECode KFitBase::addParticle(const Particle* particle)
61 {
62  return addTrack(
63  ROOTToCLHEP::getHepLorentzVector(particle->get4Vector()),
64  ROOTToCLHEP::getPoint3D(particle->getVertex()),
65  ROOTToCLHEP::getHepSymMatrix(particle->getMomentumVertexErrorMatrix()),
66  particle->getCharge());
67 }
68 
69 
70 enum KFitError::ECode
71 KFitBase::setCorrelation(const HepMatrix& e) {
72  if (e.num_row() != KFitConst::kNumber7)
73  {
74  m_ErrorCode = KFitError::kBadMatrixSize;
75  KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
76  return m_ErrorCode;
77  }
78  m_BeforeCorrelation.push_back(e);
79  m_FlagCorrelation = true;
80 
81  return m_ErrorCode = KFitError::kNoError;
82 }
83 
84 
85 enum KFitError::ECode
86 KFitBase::setZeroCorrelation() {
87  HepMatrix zero(KFitConst::kNumber7, KFitConst::kNumber7, 0);
88 
89  return this->setCorrelation(zero);
90 }
91 
92 
93 enum KFitError::ECode
94 KFitBase::setMagneticField(const double mf) {
95  m_MagneticField = mf;
96 
97  return m_ErrorCode = KFitError::kNoError;
98 }
99 
100 
101 enum KFitError::ECode
102 KFitBase::getErrorCode() const {
103  return m_ErrorCode;
104 }
105 
106 
107 int
108 KFitBase::getTrackCount() const
109 {
110  return m_TrackCount;
111 }
112 
113 
114 int
115 KFitBase::getNDF() const
116 {
117  return m_NDF;
118 }
119 
120 
121 double
122 KFitBase::getCHIsq() const
123 {
124  return m_CHIsq;
125 }
126 
127 
128 double
129 KFitBase::getMagneticField() const
130 {
131  return m_MagneticField;
132 }
133 
134 
135 double
136 KFitBase::getTrackCHIsq(const int id) const
137 {
138  if (!isFitted()) return -1.;
139  if (!isTrackIDInRange(id)) return -1.;
140 
141  HepMatrix da(m_Tracks[id].getFitParameter(KFitConst::kBeforeFit) - m_Tracks[id].getFitParameter(KFitConst::kAfterFit));
142  int err_inverse = 0;
143  const double chisq = (da.T() * (m_Tracks[id].getFitError(KFitConst::kBeforeFit).inverse(err_inverse)) * da)[0][0];
144 
145  if (err_inverse) {
146  KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kCannotGetMatrixInverse);
147  return -1.;
148  }
149 
150  return chisq;
151 }
152 
153 
154 const HepLorentzVector
155 KFitBase::getTrackMomentum(const int id) const
156 {
157  if (!isTrackIDInRange(id)) return HepLorentzVector();
158  return m_Tracks[id].getMomentum();
159 }
160 
161 const HepPoint3D
162 KFitBase::getTrackPosition(const int id) const
163 {
164  if (!isTrackIDInRange(id)) return HepPoint3D();
165  return m_Tracks[id].getPosition();
166 }
167 
168 const HepSymMatrix
169 KFitBase::getTrackError(const int id) const
170 {
171  if (!isTrackIDInRange(id)) return HepSymMatrix(KFitConst::kNumber7, 0);
172  return m_Tracks[id].getError();
173 }
174 
175 const KFitTrack
176 KFitBase::getTrack(const int id) const
177 {
178  if (!isTrackIDInRange(id)) return KFitTrack();
179  return m_Tracks[id];
180 }
181 
182 
183 const HepMatrix
184 KFitBase::getCorrelation(const int id1, const int id2, const int flag) const
185 {
186  if (flag == KFitConst::kAfterFit && !isFitted()) return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
187  if (!isTrackIDInRange(id1)) return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
188  if (!isTrackIDInRange(id2)) return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
189 
190  switch (flag) {
191  case KFitConst::kAfterFit:
192  return makeError1(
193  getTrackMomentum(id1),
194  getTrackMomentum(id2),
195  m_V_al_1.sub(KFitConst::kNumber6 * id1 + 1, KFitConst::kNumber6 * (id1 + 1), KFitConst::kNumber6 * id2 + 1,
196  KFitConst::kNumber6 * (id2 + 1))
197  );
198 
199  default:
200  if (id1 == id2) {
201 
202  return static_cast<HepMatrix>(m_Tracks[id1].getError(KFitConst::kBeforeFit));
203 
204  } else {
205  const int idx1 = id1 < id2 ? id1 : id2, idx2 = id1 < id2 ? id2 : id1;
206 
207  int index = 0;
208 
209  for (int i = 0; i < idx1; i++) index += m_TrackCount - 1 - i;
210  index -= idx1 + 1;
211  index += idx2;
212  if (id1 == idx1)
213  return m_BeforeCorrelation[index + idx2];
214  else
215  return m_BeforeCorrelation[index + idx2].T();
216  }
217  }
218 }
219 
220 
221 const HepSymMatrix
222 KFitBase::makeError1(const HepLorentzVector& p, const HepMatrix& e) const
223 {
224  // self track
225  // Error(6x6,e) ==> Error(7x7,output(hsm)) using Momentum(p).
226 
227  if (!isNonZeroEnergy(p)) return HepSymMatrix(KFitConst::kNumber7, 0);
228 
229  HepSymMatrix hsm(KFitConst::kNumber7, 0);
230 
231  for (int i = 0; i < 3; i++) for (int j = i; j < 3; j++) {
232  hsm[i][j] = e[i][j];
233  hsm[4 + i][4 + j] = e[3 + i][3 + j];
234  }
235  for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) {
236  hsm[i][4 + j] = e[i][3 + j];
237  }
238 
239  const double invE = 1 / p.t();
240  hsm[0][3] = (p.x() * hsm[0][0] + p.y() * hsm[0][1] + p.z() * hsm[0][2]) * invE;
241  hsm[1][3] = (p.x() * hsm[0][1] + p.y() * hsm[1][1] + p.z() * hsm[1][2]) * invE;
242  hsm[2][3] = (p.x() * hsm[0][2] + p.y() * hsm[1][2] + p.z() * hsm[2][2]) * invE;
243  hsm[3][3] = (p.x() * p.x() * hsm[0][0] + p.y() * p.y() * hsm[1][1] + p.z() * p.z() * hsm[2][2]
244  + 2.0 * p.x() * p.y() * hsm[0][1]
245  + 2.0 * p.x() * p.z() * hsm[0][2]
246  + 2.0 * p.y() * p.z() * hsm[1][2]) * invE * invE;
247  hsm[3][4] = (p.x() * hsm[0][4] + p.y() * hsm[1][4] + p.z() * hsm[2][4]) * invE;
248  hsm[3][5] = (p.x() * hsm[0][5] + p.y() * hsm[1][5] + p.z() * hsm[2][5]) * invE;
249  hsm[3][6] = (p.x() * hsm[0][6] + p.y() * hsm[1][6] + p.z() * hsm[2][6]) * invE;
250 
251  return hsm;
252 }
253 
254 
255 const HepMatrix
256 KFitBase::makeError1(const HepLorentzVector& p1, const HepLorentzVector& p2, const HepMatrix& e) const
257 {
258  // track and track
259  // Error(6x6,e) ==> Error(7x7,output(hm)) using Momentum(p1&p2).
260 
261  if (!isNonZeroEnergy(p1)) return HepSymMatrix(KFitConst::kNumber7, 0);
262  if (!isNonZeroEnergy(p2)) return HepSymMatrix(KFitConst::kNumber7, 0);
263 
264  HepMatrix hm(KFitConst::kNumber7, KFitConst::kNumber7, 0);
265 
266  for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) {
267  hm[i][j] = e[i][j];
268  hm[4 + i][4 + j] = e[3 + i][3 + j];
269  hm[4 + i][j] = e[3 + i][j];
270  hm[i][4 + j] = e[i][3 + j];
271  }
272 
273  const double invE1 = 1 / p1.t();
274  const double invE2 = 1 / p2.t();
275  hm[0][3] = (p2.x() * hm[0][0] + p2.y() * hm[0][1] + p2.z() * hm[0][2]) * invE2;
276  hm[1][3] = (p2.x() * hm[1][0] + p2.y() * hm[1][1] + p2.z() * hm[1][2]) * invE2;
277  hm[2][3] = (p2.x() * hm[2][0] + p2.y() * hm[2][1] + p2.z() * hm[2][2]) * invE2;
278  hm[4][3] = (p2.x() * hm[4][0] + p2.y() * hm[4][1] + p2.z() * hm[4][2]) * invE2;
279  hm[5][3] = (p2.x() * hm[5][0] + p2.y() * hm[5][1] + p2.z() * hm[5][2]) * invE2;
280  hm[6][3] = (p2.x() * hm[6][0] + p2.y() * hm[6][1] + p2.z() * hm[6][2]) * invE2;
281  hm[3][3] = (p1.x() * p2.x() * hm[0][0] + p1.y() * p2.y() * hm[1][1] + p1.z() * p2.z() * hm[2][2] +
282  p1.x() * p2.y() * hm[0][1] + p2.x() * p1.y() * hm[1][0] +
283  p1.x() * p2.z() * hm[0][2] + p2.x() * p1.z() * hm[2][0] +
284  p1.y() * p2.z() * hm[1][2] + p2.y() * p1.z() * hm[2][1]) * invE1 * invE2;
285  hm[3][0] = (p1.x() * hm[0][0] + p1.y() * hm[1][0] + p1.z() * hm[2][0]) * invE1;
286  hm[3][1] = (p1.x() * hm[0][1] + p1.y() * hm[1][1] + p1.z() * hm[2][1]) * invE1;
287  hm[3][2] = (p1.x() * hm[0][2] + p1.y() * hm[1][2] + p1.z() * hm[2][2]) * invE1;
288  hm[3][4] = (p1.x() * hm[0][4] + p1.y() * hm[1][4] + p1.z() * hm[2][4]) * invE1;
289  hm[3][5] = (p1.x() * hm[0][5] + p1.y() * hm[1][5] + p1.z() * hm[2][5]) * invE1;
290  hm[3][6] = (p1.x() * hm[0][6] + p1.y() * hm[1][6] + p1.z() * hm[2][6]) * invE1;
291 
292  return hm;
293 }
294 
295 
296 const HepMatrix
297 KFitBase::makeError2(const HepLorentzVector& p, const HepMatrix& e) const
298 {
299  // vertex and track
300  // Error(3x6,e) ==> Error(3x7,output(hm)) using Momentum(p).
301 
302  if (!isNonZeroEnergy(p)) return HepSymMatrix(KFitConst::kNumber7, 0);
303 
304  HepMatrix hm(3, KFitConst::kNumber7, 0);
305 
306  for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) {
307  hm[i][j] = e[i][j];
308  hm[i][4 + j] = e[i][3 + j];
309  }
310 
311  const double invE = 1 / p.t();
312  hm[0][3] = (p.x() * hm[0][0] + p.y() * hm[0][1] + p.z() * hm[0][2]) * invE;
313  hm[1][3] = (p.x() * hm[1][0] + p.y() * hm[1][1] + p.z() * hm[1][2]) * invE;
314  hm[2][3] = (p.x() * hm[2][0] + p.y() * hm[2][1] + p.z() * hm[2][2]) * invE;
315 
316  return hm;
317 }
318 
319 
320 const HepSymMatrix
321 KFitBase::makeError3(const HepLorentzVector& p, const HepMatrix& e, const bool is_fix_mass) const
322 {
323  // self track
324  // Error(7x7,e) ==> Error(7x7,output(hsm)) using Momentum(p).
325  // is_fix_mass = 1 : Energy term is recalculated from the other parameters.
326  // is_fix_mass = 0 : hsm = e.
327 
328  if (!isNonZeroEnergy(p)) return HepSymMatrix(KFitConst::kNumber7, 0);
329 
330  if (!is_fix_mass) {
331  HepSymMatrix hsm(KFitConst::kNumber7, 0);
332  for (int i = 0; i < 7; i++) for (int j = i; j < 7; j++) {
333  hsm[i][j] = e[i][j];
334  }
335  return hsm;
336  }
337 
338  HepSymMatrix hsm(KFitConst::kNumber7, 0);
339 
340  for (int i = 0; i < 7; i++) {
341  if (i != 3)
342  for (int j = i; j < 7; j++) hsm[i][j] = e[i][j];
343  }
344 
345  double invE = 1 / p.t();
346  hsm[0][3] = (p.x() * hsm[0][0] + p.y() * hsm[0][1] + p.z() * hsm[0][2]) * invE;
347  hsm[1][3] = (p.x() * hsm[0][1] + p.y() * hsm[1][1] + p.z() * hsm[1][2]) * invE;
348  hsm[2][3] = (p.x() * hsm[0][2] + p.y() * hsm[1][2] + p.z() * hsm[2][2]) * invE;
349  hsm[3][3] = (p.x() * p.x() * hsm[0][0] + p.y() * p.y() * hsm[1][1] + p.z() * p.z() * hsm[2][2]
350  + 2.0 * p.x() * p.y() * hsm[0][1]
351  + 2.0 * p.x() * p.z() * hsm[0][2]
352  + 2.0 * p.y() * p.z() * hsm[1][2]) * invE * invE;
353  hsm[3][4] = (p.x() * hsm[0][4] + p.y() * hsm[1][4] + p.z() * hsm[2][4]) * invE;
354  hsm[3][5] = (p.x() * hsm[0][5] + p.y() * hsm[1][5] + p.z() * hsm[2][5]) * invE;
355  hsm[3][6] = (p.x() * hsm[0][6] + p.y() * hsm[1][6] + p.z() * hsm[2][6]) * invE;
356 
357  return hsm;
358 }
359 
360 
361 const HepMatrix
362 KFitBase::makeError3(const HepLorentzVector& p1, const HepLorentzVector& p2, const HepMatrix& e, const bool is_fix_mass1,
363  const bool is_fix_mass2) const
364 {
365  // track and track
366  // Error(7x7,e) ==> Error(7x7,output(hm)) using Momentum(p1&p2).
367  // is_fix_mass = 1 : Energy term is recalculated from the other parameters.
368  // is_fix_mass = 0 : not.
369 
370  if (is_fix_mass1 && is_fix_mass2) {
371  if (!isNonZeroEnergy(p1)) return HepSymMatrix(KFitConst::kNumber7, 0);
372  if (!isNonZeroEnergy(p2)) return HepSymMatrix(KFitConst::kNumber7, 0);
373 
374  HepMatrix hm(e);
375 
376  const double invE1 = 1 / p1.t();
377  const double invE2 = 1 / p2.t();
378  hm[0][3] = (p2.x() * hm[0][0] + p2.y() * hm[0][1] + p2.z() * hm[0][2]) * invE2;
379  hm[1][3] = (p2.x() * hm[1][0] + p2.y() * hm[1][1] + p2.z() * hm[1][2]) * invE2;
380  hm[2][3] = (p2.x() * hm[2][0] + p2.y() * hm[2][1] + p2.z() * hm[2][2]) * invE2;
381  hm[4][3] = (p2.x() * hm[4][0] + p2.y() * hm[4][1] + p2.z() * hm[4][2]) * invE2;
382  hm[5][3] = (p2.x() * hm[5][0] + p2.y() * hm[5][1] + p2.z() * hm[5][2]) * invE2;
383  hm[6][3] = (p2.x() * hm[6][0] + p2.y() * hm[6][1] + p2.z() * hm[6][2]) * invE2;
384  hm[3][0] = (p1.x() * hm[0][0] + p1.y() * hm[1][0] + p1.z() * hm[2][0]) * invE1;
385  hm[3][1] = (p1.x() * hm[0][1] + p1.y() * hm[1][1] + p1.z() * hm[2][1]) * invE1;
386  hm[3][2] = (p1.x() * hm[0][2] + p1.y() * hm[1][2] + p1.z() * hm[2][2]) * invE1;
387  hm[3][3] = (p1.x() * p2.x() * hm[0][0] + p1.y() * p2.y() * hm[1][1] + p1.z() * p2.z() * hm[2][2] +
388  p1.x() * p2.y() * hm[0][1] + p2.x() * p1.y() * hm[1][0] +
389  p1.x() * p2.z() * hm[0][2] + p2.x() * p1.z() * hm[2][0] +
390  p1.y() * p2.z() * hm[1][2] + p2.y() * p1.z() * hm[2][1]) * invE1 * invE2;
391  hm[3][4] = (p1.x() * hm[0][4] + p1.y() * hm[1][4] + p1.z() * hm[2][4]) * invE1;
392  hm[3][5] = (p1.x() * hm[0][5] + p1.y() * hm[1][5] + p1.z() * hm[2][5]) * invE1;
393  hm[3][6] = (p1.x() * hm[0][6] + p1.y() * hm[1][6] + p1.z() * hm[2][6]) * invE1;
394 
395  return hm;
396  }
397 
398 
399  if (is_fix_mass1 && !is_fix_mass2) {
400  if (!isNonZeroEnergy(p1)) return HepSymMatrix(KFitConst::kNumber7, 0);
401 
402  HepMatrix hm(e);
403 
404  const double invE1 = 1 / p1.t();
405  hm[3][0] = (p1.x() * hm[0][0] + p1.y() * hm[1][0] + p1.z() * hm[2][0]) * invE1;
406  hm[3][1] = (p1.x() * hm[0][1] + p1.y() * hm[1][1] + p1.z() * hm[2][1]) * invE1;
407  hm[3][2] = (p1.x() * hm[0][2] + p1.y() * hm[1][2] + p1.z() * hm[2][2]) * invE1;
408  hm[3][3] = (p1.x() * hm[0][3] + p1.y() * hm[1][3] + p1.z() * hm[2][3]) * invE1;
409  hm[3][4] = (p1.x() * hm[0][4] + p1.y() * hm[1][4] + p1.z() * hm[2][4]) * invE1;
410  hm[3][5] = (p1.x() * hm[0][5] + p1.y() * hm[1][5] + p1.z() * hm[2][5]) * invE1;
411  hm[3][6] = (p1.x() * hm[0][6] + p1.y() * hm[1][6] + p1.z() * hm[2][6]) * invE1;
412 
413  return hm;
414  }
415 
416 
417  if (!is_fix_mass1 && is_fix_mass2) {
418  if (!isNonZeroEnergy(p2)) return HepSymMatrix(KFitConst::kNumber7, 0);
419 
420  HepMatrix hm(e);
421 
422  const double invE2 = 1 / p2.t();
423  hm[0][3] = (p2.x() * hm[0][0] + p2.y() * hm[0][1] + p2.z() * hm[0][2]) * invE2;
424  hm[1][3] = (p2.x() * hm[1][0] + p2.y() * hm[1][1] + p2.z() * hm[1][2]) * invE2;
425  hm[2][3] = (p2.x() * hm[2][0] + p2.y() * hm[2][1] + p2.z() * hm[2][2]) * invE2;
426  hm[3][3] = (p2.x() * hm[3][0] + p2.y() * hm[3][1] + p2.z() * hm[3][2]) * invE2;
427  hm[4][3] = (p2.x() * hm[4][0] + p2.y() * hm[4][1] + p2.z() * hm[4][2]) * invE2;
428  hm[5][3] = (p2.x() * hm[5][0] + p2.y() * hm[5][1] + p2.z() * hm[5][2]) * invE2;
429  hm[6][3] = (p2.x() * hm[6][0] + p2.y() * hm[6][1] + p2.z() * hm[6][2]) * invE2;
430 
431  return hm;
432  }
433 
434  return e;
435 }
436 
437 
438 const HepMatrix
439 KFitBase::makeError4(const HepLorentzVector& p, const HepMatrix& e) const
440 {
441  // vertex and track
442  // Error(3x7,e) ==> Error(3x7,output(hm)) using Momentum(p).
443  // Energy term is recalculated from the other parameters.
444 
445  if (!isNonZeroEnergy(p)) return HepSymMatrix(KFitConst::kNumber7, 0);
446 
447  HepMatrix hm(e);
448 
449  const double invE = 1 / p.t();
450  hm[0][3] = (p.x() * hm[0][0] + p.y() * hm[0][1] + p.z() * hm[0][2]) * invE;
451  hm[1][3] = (p.x() * hm[1][0] + p.y() * hm[1][1] + p.z() * hm[1][2]) * invE;
452  hm[2][3] = (p.x() * hm[2][0] + p.y() * hm[2][1] + p.z() * hm[2][2]) * invE;
453 
454  return hm;
455 }
456 
457 
458 enum KFitError::ECode
459 KFitBase::prepareCorrelation() {
460  if (m_BeforeCorrelation.size() != (double)m_TrackCount * ((double)m_TrackCount - 1)*.5)
461  {
462  m_ErrorCode = KFitError::kBadCorrelationSize;
463  KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
464  return m_ErrorCode;
465  }
466 
467  HepMatrix tmp_hm(KFitConst::kNumber6, KFitConst::kNumber6, 0);
468  int row = 0, col = 0;
469 
470  for (auto& hm : m_BeforeCorrelation)
471  {
472  row++;
473  if (row == m_TrackCount) {
474  col++;
475  row = col + 1;
476  }
477 
478  // 7x7 --> 6x6
479  for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) {
480  tmp_hm[i][j] = hm[i][j];
481  tmp_hm[3 + i][3 + j] = hm[4 + i][4 + j];
482  tmp_hm[3 + i][j] = hm[4 + i][j];
483  tmp_hm[i][3 + j] = hm[i][4 + j];
484  }
485 
486  int ii = 0, jj = 0;
487  for (int i = KFitConst::kNumber6 * row; i < KFitConst::kNumber6 * (row + 1); i++) {
488  for (int j = KFitConst::kNumber6 * col; j < KFitConst::kNumber6 * (col + 1); j++) {
489  m_V_al_0[i][j] = tmp_hm[ii][jj];
490  jj++;
491  }
492  jj = 0;
493  ii++;
494  }
495  }
496 
497  return m_ErrorCode = KFitError::kNoError;
498 }
499 
500 
501 enum KFitError::ECode
502 KFitBase::doFit1() {
503  if (m_ErrorCode != KFitError::kNoError) return m_ErrorCode;
504 
505  if (m_TrackCount < m_NecessaryTrackCount)
506  {
507  m_ErrorCode = KFitError::kBadTrackSize;
508  KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
509  return m_ErrorCode;
510  }
511 
512  if (prepareInputMatrix() != KFitError::kNoError) return m_ErrorCode;
513  if (calculateNDF() != KFitError::kNoError) return m_ErrorCode;
514 
515 
516  double chisq = 0;
517  double tmp_chisq = KFitConst::kInitialCHIsq;
518  int err_inverse = 0;
519 
520  HepMatrix tmp_al_1(m_al_1);
521  HepMatrix tmp_V_al_1(m_V_al_1);
522 
523  m_al_a = m_al_0;
524  HepMatrix tmp_al_a(m_al_a);
525 
526 
527  for (int i = 0; i < KFitConst::kMaxIterationCount; i++)
528  {
529  if (makeCoreMatrix() != KFitError::kNoError) return m_ErrorCode;
530 
531  m_V_D = (m_V_al_0.similarity(m_D)).inverse(err_inverse);
532  if (err_inverse != 0) {
533  m_ErrorCode = KFitError::kCannotGetMatrixInverse;
534  return m_ErrorCode;
535  }
536 
537  m_lam = m_V_D * (m_D * (m_al_0 - m_al_1) + m_d);
538  chisq = ((m_lam.T()) * (m_D * (m_al_0 - m_al_1) + m_d))(1, 1);
539  m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
540  m_V_al_1 = m_V_al_0 - m_V_al_0 * (m_D.T()) * m_V_D * m_D * m_V_al_0;
541 
542  if (tmp_chisq <= chisq) {
543  if (i == 0) {
544  m_ErrorCode = KFitError::kBadInitialCHIsq;
545  return m_ErrorCode;
546  } else {
547  chisq = tmp_chisq;
548  m_al_1 = tmp_al_1;
549  m_al_a = tmp_al_a;
550  m_V_al_1 = tmp_V_al_1;
551  break;
552  }
553  } else {
554  tmp_chisq = chisq;
555  tmp_al_a = tmp_al_1;
556  tmp_al_1 = m_al_1;
557  tmp_V_al_1 = m_V_al_1;
558  if (i == KFitConst::kMaxIterationCount - 1) {
559  m_al_a = tmp_al_1;
560  m_FlagOverIteration = true;
561  }
562  }
563  }
564 
565  if (m_ErrorCode != KFitError::kNoError) return m_ErrorCode;
566 
567  if (prepareOutputMatrix() != KFitError::kNoError) return m_ErrorCode;
568 
569  m_CHIsq = chisq;
570 
571  m_FlagFitted = true;
572 
573  return m_ErrorCode = KFitError::kNoError;
574 }
575 
576 
577 enum KFitError::ECode
578 KFitBase::doFit2() {
579  if (m_ErrorCode != KFitError::kNoError) return m_ErrorCode;
580 
581  if (m_TrackCount < m_NecessaryTrackCount)
582  {
583  m_ErrorCode = KFitError::kBadTrackSize;
584  KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
585  return m_ErrorCode;
586  }
587 
588  if (prepareInputMatrix() != KFitError::kNoError) return m_ErrorCode;
589  if (calculateNDF() != KFitError::kNoError) return m_ErrorCode;
590 
591 
592  double chisq = 0;
593  double tmp2_chisq = KFitConst::kInitialCHIsq;
594  int err_inverse = 0;
595 
596  m_al_a = m_al_0;
597  HepMatrix tmp_al_a(m_al_a);
598 
599  HepMatrix tmp_D(m_D), tmp_E(m_E);
600  HepMatrix tmp_V_D(m_V_D), tmp_V_E(m_V_E);
601  HepMatrix tmp_lam0(m_lam0), tmp_v_a(m_v_a);
602 
603  HepMatrix tmp2_D(m_D), tmp2_E(m_E);
604  HepMatrix tmp2_V_D(m_V_D), tmp2_V_E(m_V_E);
605  HepMatrix tmp2_lam0(m_lam0), tmp2_v_a(m_v_a), tmp2_v(m_v_a);
606 
607 
608  for (int j = 0; j < KFitConst::kMaxIterationCount; j++) // j'th loop start
609  {
610 
611  double tmp_chisq = KFitConst::kInitialCHIsq;
612 
613  for (int i = 0; i < KFitConst::kMaxIterationCount; i++) { // i'th loop start
614 
615  if (prepareInputSubMatrix() != KFitError::kNoError) return m_ErrorCode;
616  if (makeCoreMatrix() != KFitError::kNoError) return m_ErrorCode;
617 
618  m_V_D = (m_V_al_0.similarity(m_D)).inverse(err_inverse);
619  if (err_inverse) {
620  m_ErrorCode = KFitError::kCannotGetMatrixInverse;
621  return m_ErrorCode;
622  }
623 
624  m_V_E = ((m_E.T()) * m_V_D * m_E).inverse(err_inverse);
625  if (err_inverse) {
626  m_ErrorCode = KFitError::kCannotGetMatrixInverse;
627  return m_ErrorCode;
628  }
629  m_lam0 = m_V_D * (m_D * (m_al_0 - m_al_1) + m_d);
630  chisq = ((m_lam0.T()) * (m_D * (m_al_0 - m_al_1) + m_E * (m_v - m_v_a) + m_d))(1, 1);
631  m_v_a = m_v_a - m_V_E * (m_E.T()) * m_lam0;
632 
633  if (tmp_chisq <= chisq) {
634  if (i == 0) {
635  m_ErrorCode = KFitError::kBadInitialCHIsq;
636  return m_ErrorCode;
637  } else {
638  chisq = tmp_chisq;
639  m_v_a = tmp_v_a;
640  m_V_E = tmp_V_E;
641  m_V_D = tmp_V_D;
642  m_lam0 = tmp_lam0;
643  m_E = tmp_E;
644  m_D = tmp_D;
645  break;
646  }
647  } else {
648  tmp_chisq = chisq;
649  tmp_v_a = m_v_a;
650  tmp_V_E = m_V_E;
651  tmp_V_D = m_V_D;
652  tmp_lam0 = m_lam0;
653  tmp_E = m_E;
654  tmp_D = m_D;
655  if (i == KFitConst::kMaxIterationCount - 1) {
656  m_FlagOverIteration = true;
657  }
658  }
659  } // i'th loop over
660 
661 
662  m_al_a = m_al_1;
663  m_lam = m_lam0 - m_V_D * m_E * m_V_E * (m_E.T()) * m_lam0;
664  m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
665 
666  if (j == 0) {
667 
668  tmp2_chisq = chisq;
669  tmp2_v_a = m_v_a;
670  tmp2_v = m_v;
671  tmp2_V_E = m_V_E;
672  tmp2_V_D = m_V_D;
673  tmp2_lam0 = m_lam0;
674  tmp2_E = m_E;
675  tmp2_D = m_D;
676  tmp_al_a = m_al_a;
677 
678  } else {
679 
680  if (tmp2_chisq <= chisq) {
681  chisq = tmp2_chisq;
682  m_v_a = tmp2_v_a;
683  m_v = tmp2_v;
684  m_V_E = tmp2_V_E;
685  m_V_D = tmp2_V_D;
686  m_lam0 = tmp2_lam0;
687  m_E = tmp2_E;
688  m_D = tmp2_D;
689  m_al_a = tmp_al_a;
690  break;
691  } else {
692  tmp2_chisq = chisq;
693  tmp2_v_a = m_v_a;
694  tmp2_v = m_v;
695  tmp2_V_E = m_V_E;
696  tmp2_V_D = m_V_D;
697  tmp2_lam0 = m_lam0;
698  tmp2_E = m_E;
699  tmp2_D = m_D;
700  tmp_al_a = m_al_a;
701  if (j == KFitConst::kMaxIterationCount - 1) {
702  m_FlagOverIteration = true;
703  }
704  }
705  }
706  } // j'th loop over
707 
708 
709  if (m_ErrorCode != KFitError::kNoError) return m_ErrorCode;
710 
711  m_lam = m_lam0 - m_V_D * m_E * m_V_E * (m_E.T()) * m_lam0;
712  m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
713  m_V_Dt = m_V_D - m_V_D * m_E * m_V_E * (m_E.T()) * m_V_D;
714  m_V_al_1 = m_V_al_0 - m_V_al_0 * (m_D.T()) * m_V_Dt * m_D * m_V_al_0;
715  m_Cov_v_al_1 = -m_V_E * (m_E.T()) * m_V_D * m_D * m_V_al_0;
716 
717  if (prepareOutputMatrix() != KFitError::kNoError) return m_ErrorCode;
718 
719  m_CHIsq = chisq;
720 
721  m_FlagFitted = true;
722 
723  return m_ErrorCode = KFitError::kNoError;
724 }
725 
726 
727 bool
728 KFitBase::isFitted() const
729 {
730  if (m_FlagFitted) return true;
731 
732  KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kNotFittedYet);
733 
734  return false;
735 }
736 
737 
738 bool
739 KFitBase::isTrackIDInRange(const int id) const
740 {
741  if (0 <= id && id < m_TrackCount) return true;
742 
743  KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kOutOfRange);
744 
745  return false;
746 }
747 
748 
749 bool
750 KFitBase::isNonZeroEnergy(const HepLorentzVector& p) const
751 {
752  if (p.t() != 0) return true;
753 
754  KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kDivisionByZero);
755 
756  return false;
757 }
prepareAsicCrosstalkSimDB.e
e
aux.
Definition: prepareAsicCrosstalkSimDB.py:53
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19
Belle2::analysis::KFitTrack
KFitTrack is a container of the track information (Lorentz vector, position, and error matrix),...
Definition: KFitTrack.h:39
Belle2::Particle
Class to store reconstructed particles.
Definition: Particle.h:77
Belle2::analysis::KFitError::ECode
ECode
ECode is a error code enumerate.
Definition: KFitError.h:43
HepGeom::Point3D< double >