Belle II Software light-2406-ragdoll
VertexFitKFit.cc
1/**************************************************************************
2 * basf2 (Belle II Analysis Software Framework) *
3 * Author: The Belle II Collaboration *
4 * External Contributor: J. Tanaka *
5 * *
6 * See git log for contributors and copyright holders. *
7 * This file is licensed under LGPL-3.0, see LICENSE.md. *
8 **************************************************************************/
9
10
11#include <algorithm>
12#include <cstdio>
13
14#include <TMatrixFSym.h>
15
16#include <analysis/VertexFitting/KFit/MakeMotherKFit.h>
17#include <analysis/VertexFitting/KFit/VertexFitKFit.h>
18#include <analysis/utility/CLHEPToROOT.h>
19#include <framework/gearbox/Const.h>
20
21using namespace std;
22using namespace Belle2;
23using namespace Belle2::analysis;
24using namespace CLHEP;
25
27 m_BeforeVertex(HepPoint3D(0, 0, 0)),
28 m_AfterVertexError(HepSymMatrix(3, 0)),
29 m_BeamError(HepSymMatrix(3, 0))
30{
31 m_CorrelationMode = false;
32 m_FlagFitted = false;
33 m_FlagKnownVertex = false;
34 m_FlagBeam = false;
35 m_FlagTube = false;
36 m_iTrackTube = -1;
37 m_CHIsqVertex = 0;
39 m_V_E = HepMatrix(3, 3, 0);
40 m_v = HepMatrix(3, 1, 0);
41 m_v_a = HepMatrix(3, 1, 0);
42
44}
45
47
48
50VertexFitKFit::setInitialVertex(const HepPoint3D& v) {
52
54}
55
57{
58 m_BeforeVertex = HepPoint3D(v.X(), v.Y(), v.Z());
60 return m_ErrorCode;
61}
62
64VertexFitKFit::setIpProfile(const HepPoint3D& ip, const HepSymMatrix& ipe) {
65 if (m_FlagTube)
66 {
67 char buf[1024];
68 sprintf(buf, "%s:%s(): already constrained to IPtube", __FILE__, __func__);
69 B2FATAL(buf);
70 }
71
72 m_FlagBeam = true;
73 m_BeforeVertex = ip;
74 m_BeamError = ipe;
75
77}
78
79
81VertexFitKFit::setIpTubeProfile(const HepLorentzVector& p, const HepPoint3D& x, const HepSymMatrix& e, const double q) {
82 if (m_FlagBeam)
83 {
84 char buf[1024];
85 sprintf(buf, "%s:%s(): already constrained to IP", __FILE__, __func__);
86 B2FATAL(buf);
87 }
88
89 m_FlagTube = true;
90 m_TubeTrack = KFitTrack(p, x, e, q);
91
93}
94
95
98 m_FlagKnownVertex = flag;
99
101}
102
103
107
109}
110
111
112const HepPoint3D
113VertexFitKFit::getVertex(const int flag) const
114{
115 if (flag == KFitConst::kAfterFit && !isFitted()) return HepPoint3D();
116
117 switch (flag) {
119 return m_BeforeVertex;
120
122 return m_AfterVertex;
123
124 default:
125 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kOutOfRange);
126 return HepPoint3D();
127 }
128}
129
130
131const HepSymMatrix
133{
134 return m_AfterVertexError;
135}
136
137
138double
140{
141 return KFitBase::getCHIsq();
142}
143
144
145double
147{
148 // only m_FlagBeam = 1
149 return m_CHIsqVertex;
150}
151
152
153const HepMatrix
155{
156 if (!isTrackIDInRange(id)) return HepMatrix(3, KFitConst::kNumber7, 0);
157 return m_AfterTrackVertexError[id];
158}
159
160
161double
163{
164 if (!isTrackIDInRange(id)) return -1;
165
167 return KFitBase::getTrackCHIsq(id);
168 }
169
170 return m_EachCHIsq[id];
171}
172
173
174double
176{
178 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
179 return -1;
180 }
181
182 if (m_TrackCount == 0) {
183 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kBadTrackSize);
184 return -1;
185 }
186
187 double chisq = 0.0;
188 for (int i = 0; i < m_TrackCount; i++) {
189 const double i_chisq = this->getTrackCHIsq(i);
190 chisq += i_chisq;
191 }
192
193 return chisq;
194}
195
196
197int
199{
201 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
202 return 0;
203 }
204
205 if (m_TrackCount == 0) {
206 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kBadTrackSize);
207 return 0;
208 }
209
210 return m_TrackCount * 2 - 2;
211}
212
213
216 if (m_FlagTube) this->appendTube();
217
218 if (m_FlagBeam) m_ErrorCode = this->doFit4();
219 else if (m_FlagKnownVertex) m_ErrorCode = this->doFit5();
220 else if (m_CorrelationMode) m_ErrorCode = this->doFit2();
221 else
222 m_ErrorCode = this->doFit3();
223
224 const enum KFitError::ECode tmp_ErrorCode = m_ErrorCode;
225
226 if (m_FlagTube) this->deleteTube();
227
228 if (tmp_ErrorCode == KFitError::kNoError) m_FlagFitted = true;
229
230 return m_ErrorCode = tmp_ErrorCode;
231}
232
233
236 // use small Matrix --> No Correlation
238
240 {
242 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
243 return m_ErrorCode;
244 }
245
248
249
250 double chisq = 0;
251 double tmp2_chisq = KFitConst::kInitialCHIsq;
252 int err_inverse = 0;
253
254 m_al_a = m_al_0;
255 HepMatrix tmp_al_a(m_al_a);
256
257 HepMatrix tmp_D(m_D), tmp_E(m_E);
258 HepMatrix tmp_V_D(m_V_D), tmp_V_E(m_V_E);
259 HepMatrix tmp_lam0(m_lam0), tmp_v_a(m_v_a);
260
261 HepMatrix tmp2_D(m_D), tmp2_E(m_E);
262 HepMatrix tmp2_V_D(m_V_D), tmp2_V_E(m_V_E);
263 HepMatrix tmp2_lam0(m_lam0), tmp2_v_a(m_v_a);
264
265 std::vector<double> tmp_each_chisq(m_TrackCount);
266 std::vector<double> tmp2_each_chisq(m_TrackCount);
267
268 for (int j = 0; j < KFitConst::kMaxIterationCount; j++) // j'th loop start
269 {
270
271 double tmp_chisq = KFitConst::kInitialCHIsq;
272
273 for (int i = 0; i < KFitConst::kMaxIterationCount; i++) { // i'th loop start
274
277
278 HepMatrix tV_Ein(3, 3, 0);
279 chisq = 0;
280
281 for (int k = 0; k < m_TrackCount; k++) { // k'th loop start
282
283 HepMatrix tD = m_D.sub(2 * k + 1, 2 * (k + 1), KFitConst::kNumber6 * k + 1, KFitConst::kNumber6 * (k + 1)); // 2x6
284 HepMatrix tV_D = ((m_V_al_0.sub(KFitConst::kNumber6 * k + 1,
285 (int)(KFitConst::kNumber6 * (k + 1)))).similarity(tD)).inverse(err_inverse); // 2x2
286 if (err_inverse) {
288 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
289 return m_ErrorCode;
290 }
291
292 m_V_D.sub(2 * k + 1, 2 * k + 1, tV_D);
293 HepMatrix tE = m_E.sub(2 * k + 1, 2 * (k + 1), 1, 3); // 2x3
294 tV_Ein += (tE.T()) * tV_D * tE; // 3x3
295 HepMatrix tDeltaAl = (m_al_0 - m_al_1).sub(KFitConst::kNumber6 * k + 1, KFitConst::kNumber6 * (k + 1), 1, 1); // 6x1
296 HepMatrix td = m_d.sub(2 * k + 1, 2 * (k + 1), 1, 1); // 2x1
297 HepMatrix tlam0 = tV_D * (tD * tDeltaAl + td); // 2x2x(2x6x6x1+2x1) = 2x1
298 m_lam0.sub(2 * k + 1, 1, tlam0);
299 m_EachCHIsq[k] = ((tlam0.T()) * (tD * tDeltaAl + tE * (m_v - m_v_a) + td))(1, 1); // 1x2x(2x6x6x1+2x3x3x1+2x1)
300 chisq += m_EachCHIsq[k];
301 } // k'th loop over
302
303 m_V_E = tV_Ein.inverse(err_inverse);
304 if (err_inverse) {
306 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
307 return m_ErrorCode;
308 }
309
310 m_v_a = m_v_a - m_V_E * (m_E.T()) * m_lam0;
311
312 if (tmp_chisq <= chisq) {
313 if (i == 0) {
315 } else {
316 for (int k = 0; k < m_TrackCount; k++) m_EachCHIsq[k] = tmp_each_chisq[k];
317 chisq = tmp_chisq;
318 m_v_a = tmp_v_a;
319 m_V_E = tmp_V_E;
320 m_V_D = tmp_V_D;
321 m_lam0 = tmp_lam0;
322 m_E = tmp_E;
323 m_D = tmp_D;
324 }
325 break;
326 } else {
327 for (int k = 0; k < m_TrackCount; k++) tmp_each_chisq[k] = m_EachCHIsq[k];
328 tmp_chisq = chisq;
329 tmp_v_a = m_v_a;
330 tmp_V_E = m_V_E;
331 tmp_V_D = m_V_D;
332 tmp_lam0 = m_lam0;
333 tmp_E = m_E;
334 tmp_D = m_D;
335 if (i == KFitConst::kMaxIterationCount - 1) {
336 m_FlagOverIteration = true;
337 }
338 }
339 } // i'th loop over
340
341 m_al_a = m_al_1;
342 m_lam = m_lam0 - m_V_D * m_E * m_V_E * (m_E.T()) * m_lam0;
343 m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
344
345 if (j == 0) {
346
347 for (int k = 0; k < m_TrackCount; k++) tmp2_each_chisq[k] = m_EachCHIsq[k];
348 tmp2_chisq = chisq;
349 tmp2_v_a = m_v_a;
350 tmp2_V_E = m_V_E;
351 tmp2_V_D = m_V_D;
352 tmp2_lam0 = m_lam0;
353 tmp2_E = m_E;
354 tmp2_D = m_D;
355 tmp_al_a = m_al_a;
356
357 } else {
358
359 if (tmp2_chisq <= chisq) {
360 for (int k = 0; k < m_TrackCount; k++) m_EachCHIsq[k] = tmp2_each_chisq[k];
361 chisq = tmp2_chisq;
362 m_v_a = tmp2_v_a;
363 m_V_E = tmp2_V_E;
364 m_V_D = tmp2_V_D;
365 m_lam0 = tmp2_lam0;
366 m_E = tmp2_E;
367 m_D = tmp2_D;
368 m_al_a = tmp_al_a;
369 break;
370 } else {
371 for (int k = 0; k < m_TrackCount; k++) tmp2_each_chisq[k] = m_EachCHIsq[k];
372 tmp2_chisq = chisq;
373 tmp2_v_a = m_v_a;
374 tmp2_V_E = m_V_E;
375 tmp2_V_D = m_V_D;
376 tmp2_lam0 = m_lam0;
377 tmp2_E = m_E;
378 tmp2_D = m_D;
379 tmp_al_a = m_al_a;
380 }
381 }
382 } // j'th loop over
383
384
386
387 m_lam = m_lam0 - m_V_D * m_E * m_V_E * (m_E.T()) * m_lam0;
388 m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
389 m_V_Dt = m_V_D - m_V_D * m_E * m_V_E * (m_E.T()) * m_V_D;
390 m_V_al_1 = m_V_al_0 - m_V_al_0 * (m_D.T()) * m_V_Dt * m_D * m_V_al_0;
391 m_Cov_v_al_1 = -m_V_E * (m_E.T()) * m_V_D * m_D * m_V_al_0;
392
394
395 m_CHIsq = chisq;
396
398}
399
400
403 // included beam position constraint (only no correlation)
405
407 {
409 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
410 return m_ErrorCode;
411 }
412
415
416
417 double chisq = 0;
418 double tmp_chisq = KFitConst::kInitialCHIsq;
419 int err_inverse = 0;
420
421 m_al_a = m_al_0;
422 HepMatrix tmp_al_a(m_al_a);
423
424 HepMatrix tmp_D(m_D), tmp_E(m_E);
425 HepMatrix tmp_lam(m_lam);
426
427 // vertex
428 m_v[0][0] = m_BeforeVertex.x();
429 m_v[1][0] = m_BeforeVertex.y();
430 m_v[2][0] = m_BeforeVertex.z();
431
432 std::vector<double> tmp_each_chisq(m_TrackCount);
433 double tmp_vertex_chisq = 1.e+30; // An init-value is not needed but the C++ compiler requires the init-value.
434
435 // to avoid overestimation of vertex-z error.
436 bool it_flag = false;
437
438 for (int i = 0; i < KFitConst::kMaxIterationCount ; i++)
439 {
440
442
443 chisq = 0;
444
445 HepMatrix tV_Dtin = m_V_al_0.similarity(m_D) + m_BeamError.similarity(m_E);
446 HepMatrix tV_Dt = tV_Dtin.inverse(err_inverse);
447 if (err_inverse) {
449 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
450 return m_ErrorCode;
451 }
452 m_lam = tV_Dt * (m_D * (m_al_0 - m_al_1) + m_E * (m_v - m_v_a) + m_d); // (2*nTrk)x1
453 for (int k = 0; k < m_TrackCount; k++) {
454 HepMatrix tD = m_D.sub(2 * k + 1, 2 * (k + 1), KFitConst::kNumber6 * k + 1, KFitConst::kNumber6 * (k + 1)); // 2x6
455 HepMatrix tDeltaAl = (m_al_0 - m_al_1).sub(KFitConst::kNumber6 * k + 1, KFitConst::kNumber6 * (k + 1), 1, 1); // 6x1
456 HepMatrix td = m_d.sub(2 * k + 1, 2 * (k + 1), 1, 1); // 2x1
457 HepMatrix tE = m_E.sub(2 * k + 1, 2 * (k + 1), 1, 3); // 2x3
458 chisq += ((m_lam.sub(2 * k + 1, 2 * (k + 1), 1, 1).T()) * (tD * tDeltaAl + tE * (m_v - m_v_a) + td))(1,
459 1); // 1x2x(2x6x6x1+2x3x3x1+2x1)
460 m_EachCHIsq[k] = (m_lam.sub(2 * k + 1, 2 * (k + 1), 1, 1).T() * tD * m_V_al_0.sub(KFitConst::kNumber6 * k + 1,
461 KFitConst::kNumber6 * (k + 1)) * (tD.T()) * m_lam.sub(2 * k + 1, 2 * (k + 1), 1, 1))(1, 1);
462 }
463
464 m_CHIsqVertex = (m_lam.T() * m_E * m_BeamError * (m_E.T()) * m_lam)(1, 1);
465 m_al_a = m_al_1;
466 m_v_a = m_v - m_BeamError * (m_E.T()) * m_lam;
467 m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
468
469 if (tmp_chisq <= chisq && it_flag) {
470 if (i == 0) {
472 } else {
473 for (int k = 0; k < m_TrackCount; k++) m_EachCHIsq[k] = tmp_each_chisq[k];
474 m_CHIsqVertex = tmp_vertex_chisq;
475 chisq = tmp_chisq;
476 m_lam = tmp_lam;
477 m_E = tmp_E;
478 m_D = tmp_D;
479 m_al_a = tmp_al_a;
480 }
481 break;
482 } else {
483 if (tmp_chisq <= chisq) it_flag = true;
484 for (int k = 0; k < m_TrackCount; k++) tmp_each_chisq[k] = m_EachCHIsq[k];
485 tmp_vertex_chisq = m_CHIsqVertex;
486 tmp_chisq = chisq;
487 tmp_lam = m_lam;
488 tmp_E = m_E;
489 tmp_D = m_D;
490 tmp_al_a = m_al_a;
491 if (i == KFitConst::kMaxIterationCount - 1) {
492 m_FlagOverIteration = true;
493 }
494 }
495 }
496
497
499
500 m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
501 m_v_a = m_v - m_BeamError * (m_E.T()) * m_lam;
502 HepMatrix tV_Dtin = m_V_al_0.similarity(m_D) + m_BeamError.similarity(m_E);
503 m_V_Dt = tV_Dtin.inverse(err_inverse);
504 if (err_inverse)
505 {
507 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
508 return m_ErrorCode;
509 }
510
511 m_V_al_1 = m_V_al_0 - m_V_al_0 * (m_D.T()) * m_V_Dt * m_D * m_V_al_0;
513 // m_V_v is m_V_E
514 // --> need to replace m_V_E for my implementation.
516
518
519 m_CHIsq = chisq;
520
522}
523
524
527 // known vertex --> do not find vertex. (only no correlation)
529
531 {
533 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
534 return m_ErrorCode;
535 }
536
539
540
541 double chisq = 0;
542 double tmp_chisq = KFitConst::kInitialCHIsq;
543 int err_inverse = 0;
544
545 m_al_a = m_al_0;
546 HepMatrix tmp_al_a(m_al_a);
547
548 HepMatrix tmp_al_0(m_al_1);
549 HepMatrix tmp_V_al_0(m_V_al_1);
550
551 std::vector<double> tmp_each_chisq(m_TrackCount);
552
553 for (int i = 0; i < KFitConst::kMaxIterationCount; i++)
554 {
555
557
558 chisq = 0;
559
560 for (int k = 0; k < m_TrackCount; k++) {
561 HepMatrix tD = m_D.sub(2 * k + 1, 2 * (k + 1), KFitConst::kNumber6 * k + 1, KFitConst::kNumber6 * (k + 1)); // 2x6
562 HepMatrix tV_D = ((m_V_al_0.sub(KFitConst::kNumber6 * k + 1,
563 (int)(KFitConst::kNumber6 * (k + 1)))).similarity(tD)).inverse(err_inverse); // 2x2
564 if (err_inverse) {
566 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
567 return m_ErrorCode;
568 }
569 m_V_D.sub(2 * k + 1, 2 * k + 1, tV_D);
570
571 HepMatrix tDeltaAl = (m_al_0 - m_al_1).sub(KFitConst::kNumber6 * k + 1, KFitConst::kNumber6 * (k + 1), 1, 1); // 6x1
572 HepMatrix td = m_d.sub(2 * k + 1, 2 * (k + 1), 1, 1); // 2x1
573 HepMatrix tlam = tV_D * (tD * tDeltaAl + td); // 2x2x(2x6x6x1+2x1) = 2x1
574 m_lam.sub(2 * k + 1, 1, tlam);
575 m_EachCHIsq[k] = ((tlam.T()) * (tD * tDeltaAl + td))(1, 1); // 1x2x(2x6x6x1+2x1)
576 chisq += m_EachCHIsq[k];
577 }
578
579 m_al_a = m_al_1;
580 m_al_1 = m_al_0 - m_V_al_0 * (m_D.T()) * m_lam;
581 m_V_al_1 = m_V_al_0 - m_V_al_0 * (m_D.T()) * m_V_D * m_D * m_V_al_0;
582
583 if (tmp_chisq <= chisq) {
584 if (i == 0) {
586 } else {
587 for (int k = 0; k < m_TrackCount; k++) m_EachCHIsq[k] = tmp_each_chisq[k];
588 chisq = tmp_chisq;
589 m_al_1 = tmp_al_0;
590 m_V_al_1 = tmp_V_al_0;
591 m_al_a = tmp_al_a;
592 }
593 break;
594 } else {
595 for (int k = 0; k < m_TrackCount; k++) tmp_each_chisq[k] = m_EachCHIsq[k];
596 tmp_chisq = chisq;
597 tmp_al_0 = m_al_1;
598 tmp_V_al_0 = m_V_al_1;
599 tmp_al_a = m_al_a;
600 if (i == KFitConst::kMaxIterationCount - 1) {
601 m_FlagOverIteration = true;
602 }
603 }
604 }
605
606
608
610
611 m_CHIsq = chisq;
612
614}
615
616
620 {
623 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
624 return m_ErrorCode;
625 }
626 } else
627 {
630 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
631 return m_ErrorCode;
632 }
633 }
634
635
636 int index = 0;
637 HepMatrix tmp_al_0(KFitConst::kNumber6 * m_TrackCount, 1, 0);
638 HepSymMatrix tmp_V_al_0(KFitConst::kNumber6 * m_TrackCount, 0);
639 HepMatrix tmp_property(m_TrackCount, 3, 0);
640
641
642 for (auto& track : m_Tracks)
643 {
644 // momentum x,y,z and position x,y,z
645 for (int j = 0; j < KFitConst::kNumber6; j++)
646 tmp_al_0[index * KFitConst::kNumber6 + j][0] = track.getFitParameter(j, KFitConst::kBeforeFit);
647 // these error
648 tmp_V_al_0.sub(index * KFitConst::kNumber6 + 1, track.getFitError(KFitConst::kBeforeFit));
649 // charge , mass , a
650 tmp_property[index][0] = track.getCharge();
651 tmp_property[index][1] = track.getMass();
652 const double c = Belle2::Const::speedOfLight * 1e-4;
653 tmp_property[index][2] = -c * m_MagneticField * track.getCharge();
654 index++;
655 }
656
657 // error between tarck and track
658 m_V_al_0 = tmp_V_al_0;
660 {
661 if (m_FlagCorrelation) {
662 this->prepareCorrelation();
664 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
665 return m_ErrorCode;
666 }
667 }
668 }
669
670 // vertex
671 m_v_a[0][0] = m_BeforeVertex.x();
672 m_v_a[1][0] = m_BeforeVertex.y();
673 m_v_a[2][0] = m_BeforeVertex.z();
674
675 // set member matrix
676 m_al_0 = tmp_al_0;
677 m_al_1 = m_al_0;
678 m_property = tmp_property;
679
680 // define size of matrix
683 m_E = m_V_al_1.sub(1, m_TrackCount * 2, 1, 3);
684 m_d = m_V_al_1.sub(1, m_TrackCount * 2, 1, 1);
685 m_V_D = m_V_al_1.sub(1, m_TrackCount * 2, 1, m_TrackCount * 2);
686 m_lam = m_V_al_1.sub(1, m_TrackCount * 2, 1, 1);
687 m_lam0 = m_V_al_1.sub(1, m_TrackCount * 2, 1, 1);
688 m_V_Dt = m_V_al_1.sub(1, m_TrackCount * 2, 1, m_TrackCount * 2);
690
692}
693
694
697 // vertex
698 for (int i = 0; i < 3; i++)
699 {
700 m_v[i][0] = m_v_a[i][0];
701 }
703}
704
705
708 Hep3Vector h3v;
709 unsigned index = 0;
710
711 for (auto& pdata : m_Tracks)
712 {
713 // tracks
714 // momentum
715 h3v.setX(m_al_1[index * KFitConst::kNumber6 + 0][0]);
716 h3v.setY(m_al_1[index * KFitConst::kNumber6 + 1][0]);
717 h3v.setZ(m_al_1[index * KFitConst::kNumber6 + 2][0]);
718 pdata.setMomentum(HepLorentzVector(h3v, sqrt(h3v.mag2() + pdata.getMass()*pdata.getMass())), KFitConst::kAfterFit);
719 // position
720 pdata.setPosition(HepPoint3D(
721 m_al_1[index * KFitConst::kNumber6 + 3][0],
722 m_al_1[index * KFitConst::kNumber6 + 4][0],
724 // error of the tracks
725 pdata.setError(makeError1(pdata.getMomentum(),
726 m_V_al_1.sub(
727 index * KFitConst::kNumber6 + 1,
728 (index + 1)*KFitConst::kNumber6,
729 index * KFitConst::kNumber6 + 1,
730 (index + 1)*KFitConst::kNumber6)),
732 if (m_ErrorCode != KFitError::kNoError) break;
733 index++;
734 }
735
736 // vertex
737 m_AfterVertex.setX(m_v_a[0][0]);
738 m_AfterVertex.setY(m_v_a[1][0]);
739 m_AfterVertex.setZ(m_v_a[2][0]);
740
741 // error of the vertex
742 for (int i = 0; i < 3; i++) for (int j = i; j < 3; j++)
743 {
744 m_AfterVertexError[i][j] = m_V_E[i][j];
745 }
746
747 // error between vertex and tracks
748 for (int i = 0; i < m_TrackCount; i++)
749 {
750 HepMatrix hm(3, KFitConst::kNumber6, 0);
751 for (int j = 0; j < 3; j++) for (int k = 0; k < KFitConst::kNumber6; k++) {
752 hm[j][k] = m_Cov_v_al_1[j][KFitConst::kNumber6 * i + k];
753 }
754 m_AfterTrackVertexError.push_back(makeError2(m_Tracks[i].getMomentum(), hm));
755 }
756
758}
759
760
763 // vertex fit
764 for (int i = 0; i < m_TrackCount; i++)
765 {
766 double S, U;
767 double sininv;
768
769 double px = m_al_1[i * KFitConst::kNumber6 + 0][0];
770 double py = m_al_1[i * KFitConst::kNumber6 + 1][0];
771 double pz = m_al_1[i * KFitConst::kNumber6 + 2][0];
772 double x = m_al_1[i * KFitConst::kNumber6 + 3][0];
773 double y = m_al_1[i * KFitConst::kNumber6 + 4][0];
774 double z = m_al_1[i * KFitConst::kNumber6 + 5][0];
775 double a = m_property[i][2];
776
777 double pt = sqrt(px * px + py * py);
778 if (pt == 0) {
780 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
781 return m_ErrorCode;
782 }
783
784 double invPt = 1. / pt;
785 double invPt2 = invPt * invPt;
786 double dlx = m_v_a[0][0] - x;
787 double dly = m_v_a[1][0] - y;
788 double dlz = m_v_a[2][0] - z;
789 double a1 = -dlx * py + dly * px;
790 double a2 = dlx * px + dly * py;
791 double r2d2 = dlx * dlx + dly * dly;
792 double Rx = dlx - 2.*px * a2 * invPt2;
793 double Ry = dly - 2.*py * a2 * invPt2;
794
795 if (a != 0) { // charged
796
797 double B = a * a2 * invPt2;
798 if (fabs(B) > 1) {
800 B2DEBUG(10, "KFitError: Cannot calculate arcsin");
801 //KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
802 return m_ErrorCode;
803 }
804 // sin^(-1)(B)
805 sininv = asin(B);
806 double tmp0 = 1.0 - B * B;
807 if (tmp0 == 0) {
809 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
810 return m_ErrorCode;
811 }
812 // 1/sqrt(1-B^2)
813 double sqrtag = 1.0 / sqrt(tmp0);
814 S = sqrtag * invPt2;
815 U = dlz - pz * sininv / a;
816
817 } else { // neutral
818
819 sininv = 0.0;
820 S = invPt2;
821 U = dlz - pz * a2 * invPt2;
822 }
823
824 // d
825 m_d[i * 2 + 0][0] = a1 - 0.5 * a * r2d2;
826 m_d[i * 2 + 1][0] = U * pt;
827
828 // D
829 m_D[i * 2 + 0][i * KFitConst::kNumber6 + 0] = dly;
830 m_D[i * 2 + 0][i * KFitConst::kNumber6 + 1] = -dlx;
831 m_D[i * 2 + 0][i * KFitConst::kNumber6 + 2] = 0.0;
832 m_D[i * 2 + 0][i * KFitConst::kNumber6 + 3] = py + a * dlx;
833 m_D[i * 2 + 0][i * KFitConst::kNumber6 + 4] = -px + a * dly;
834 m_D[i * 2 + 0][i * KFitConst::kNumber6 + 5] = 0.0;
835 m_D[i * 2 + 1][i * KFitConst::kNumber6 + 0] = -pz * pt * S * Rx + U * px * invPt;
836 m_D[i * 2 + 1][i * KFitConst::kNumber6 + 1] = -pz * pt * S * Ry + U * py * invPt;
837 m_D[i * 2 + 1][i * KFitConst::kNumber6 + 2] = a != 0 ? -sininv * pt / a : -a2 * invPt;
838 m_D[i * 2 + 1][i * KFitConst::kNumber6 + 3] = px * pz * pt * S;
839 m_D[i * 2 + 1][i * KFitConst::kNumber6 + 4] = py * pz * pt * S;
840 m_D[i * 2 + 1][i * KFitConst::kNumber6 + 5] = -pt;
841
842 // E
843 m_E[i * 2 + 0][0] = -py - a * dlx;
844 m_E[i * 2 + 0][1] = px - a * dly;
845 m_E[i * 2 + 0][2] = 0.0;
846 m_E[i * 2 + 1][0] = -px * pz * pt * S;
847 m_E[i * 2 + 1][1] = -py * pz * pt * S;
848 m_E[i * 2 + 1][2] = pt;
849 }
850
852}
853
854
857 if (m_FlagBeam) m_NDF = 2 * m_TrackCount;
858 else if (m_FlagTube) m_NDF = 2 * (m_TrackCount - 1) - 1;
859 else if (m_FlagKnownVertex) m_NDF = 2 * m_TrackCount;
860 else m_NDF = 2 * m_TrackCount - 3;
861
863}
864
865
869
870 if (m_iTrackTube != -1)
871 {
872 char buf[1024];
873 sprintf(buf, "%s:%s(): internal error; duplicated appendTube() call?", __FILE__, __func__);
874 B2FATAL(buf);
875 }
876
877 m_Tracks.push_back(m_TubeTrack);
878 m_TrackCount = m_Tracks.size();
880
882}
883
884
888
889 if (m_iTrackTube == -1)
890 {
891 char buf[1024];
892 sprintf(buf, "%s:%s(): internal error; duplicated deleteTube() call?", __FILE__, __func__);
893 B2FATAL(buf);
894 }
895
896 m_Tracks.pop_back();
897 m_TrackCount = m_Tracks.size();
898 m_iTrackTube = -1;
899
901}
902
904{
905 MakeMotherKFit kmm;
907 unsigned n = getTrackCount();
908 for (unsigned i = 0; i < n; ++i) {
910 getTrack(i).getCharge());
912 for (unsigned j = i + 1; j < n; ++j) {
914 }
915 }
916 kmm.setVertex(getVertex());
918 m_ErrorCode = kmm.doMake();
920 return m_ErrorCode;
921 double chi2 = getCHIsq();
922 int ndf = getNDF();
923 double prob = TMath::Prob(chi2, ndf);
924 //
925 bool haschi2 = mother->hasExtraInfo("chiSquared");
926 if (haschi2) {
927 mother->setExtraInfo("chiSquared", chi2);
928 mother->setExtraInfo("ndf", ndf);
929 } else {
930 mother->addExtraInfo("chiSquared", chi2);
931 mother->addExtraInfo("ndf", ndf);
932 }
933
934 mother->updateMomentum(
935 CLHEPToROOT::getLorentzVector(kmm.getMotherMomentum()),
936 CLHEPToROOT::getXYZVector(kmm.getMotherPosition()),
937 CLHEPToROOT::getTMatrixFSym(kmm.getMotherError()),
938 prob);
940 return m_ErrorCode;
941}
DataType Z() const
access variable Z (= .at(2) without boundary check)
Definition: B2Vector3.h:435
DataType X() const
access variable X (= .at(0) without boundary check)
Definition: B2Vector3.h:431
DataType Y() const
access variable Y (= .at(1) without boundary check)
Definition: B2Vector3.h:433
static const double speedOfLight
[cm/ns]
Definition: Const.h:695
Class to store reconstructed particles.
Definition: Particle.h:75
void setExtraInfo(const std::string &name, double value)
Sets the user-defined data of given name to the given value.
Definition: Particle.cc:1317
bool hasExtraInfo(const std::string &name) const
Return whether the extra info with the given name is set.
Definition: Particle.cc:1266
void addExtraInfo(const std::string &name, double value)
Sets the user-defined data of given name to the given value.
Definition: Particle.cc:1336
void updateMomentum(const ROOT::Math::PxPyPzEVector &p4, const ROOT::Math::XYZVector &vertex, const TMatrixFSym &errMatrix, double pValue)
Sets Lorentz vector, position, 7x7 error matrix and p-value.
Definition: Particle.h:386
int m_NecessaryTrackCount
Number needed tracks to perform fit.
Definition: KFitBase.h:303
double m_MagneticField
Magnetic field.
Definition: KFitBase.h:311
const CLHEP::HepSymMatrix makeError1(const CLHEP::HepLorentzVector &p, const CLHEP::HepMatrix &e) const
Rebuild an error matrix from a Lorentz vector and an error matrix.
Definition: KFitBase.cc:221
CLHEP::HepMatrix m_al_1
See J.Tanaka Ph.D (2001) p136 for definition.
Definition: KFitBase.h:259
CLHEP::HepMatrix m_V_Dt
See J.Tanaka Ph.D (2001) p138 for definition.
Definition: KFitBase.h:289
const CLHEP::HepSymMatrix getTrackError(const int id) const
Get an error matrix of the track.
Definition: KFitBase.cc:168
virtual double getCHIsq(void) const
Get a chi-square of the fit.
Definition: KFitBase.cc:121
const CLHEP::HepLorentzVector getTrackMomentum(const int id) const
Get a Lorentz vector of the track.
Definition: KFitBase.cc:154
CLHEP::HepMatrix m_lam
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:276
enum KFitError::ECode doFit2(void)
Perform a fit (used in VertexFitKFit::doFit() and MassVertexFitKFit::doFit()).
Definition: KFitBase.cc:578
CLHEP::HepMatrix m_E
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:279
const HepPoint3D getTrackPosition(const int id) const
Get a position of the track.
Definition: KFitBase.cc:161
bool m_FlagOverIteration
Flag whether the iteration count exceeds the limit.
Definition: KFitBase.h:308
CLHEP::HepMatrix m_property
Container of charges and masses.
Definition: KFitBase.h:263
virtual double getTrackCHIsq(const int id) const
Get a chi-square of the track.
Definition: KFitBase.cc:135
enum KFitError::ECode m_ErrorCode
Error code.
Definition: KFitBase.h:243
CLHEP::HepMatrix m_V_al_1
See J.Tanaka Ph.D (2001) p138 for definition.
Definition: KFitBase.h:274
virtual int getNDF(void) const
Get an NDF of the fit.
Definition: KFitBase.cc:114
CLHEP::HepMatrix m_d
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:268
CLHEP::HepMatrix m_lam0
See J.Tanaka Ph.D (2001) p138 for definition.
Definition: KFitBase.h:283
bool isFitted(void) const
Return false if fit is not performed yet or performed fit is failed; otherwise true.
Definition: KFitBase.cc:728
CLHEP::HepMatrix m_al_a
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:261
CLHEP::HepMatrix m_D
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:266
CLHEP::HepMatrix m_V_D
See J.Tanaka Ph.D (2001) p138 for definition.
Definition: KFitBase.h:271
bool isTrackIDInRange(const int id) const
Check if the id is in the range.
Definition: KFitBase.cc:739
CLHEP::HepMatrix m_v_a
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:287
virtual const CLHEP::HepMatrix getCorrelation(const int id1, const int id2, const int flag=KFitConst::kAfterFit) const
Get a correlation matrix between two tracks.
Definition: KFitBase.cc:183
bool m_FlagCorrelation
Flag whether a correlation among tracks exists.
Definition: KFitBase.h:306
CLHEP::HepSymMatrix m_V_al_0
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:255
CLHEP::HepMatrix m_V_E
See J.Tanaka Ph.D (2001) p138 for definition.
Definition: KFitBase.h:281
CLHEP::HepMatrix m_Cov_v_al_1
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:291
const KFitTrack getTrack(const int id) const
Get a specified track object.
Definition: KFitBase.cc:175
virtual enum KFitError::ECode prepareCorrelation(void)
Build a grand correlation matrix from input-track properties.
Definition: KFitBase.cc:459
const CLHEP::HepMatrix makeError2(const CLHEP::HepLorentzVector &p, const CLHEP::HepMatrix &e) const
Rebuild an error matrix from a Lorentz vector and an error matrix.
Definition: KFitBase.cc:296
bool m_FlagFitted
Flag to indicate if the fit is performed and succeeded.
Definition: KFitBase.h:245
double m_CHIsq
chi-square of the fit.
Definition: KFitBase.h:297
int getTrackCount(void) const
Get the number of added tracks.
Definition: KFitBase.cc:107
int m_NDF
NDF of the fit.
Definition: KFitBase.h:295
std::vector< KFitTrack > m_Tracks
Container of input tracks.
Definition: KFitBase.h:249
CLHEP::HepMatrix m_v
See J.Tanaka Ph.D (2001) p137 for definition.
Definition: KFitBase.h:285
int m_TrackCount
Number of tracks.
Definition: KFitBase.h:301
CLHEP::HepMatrix m_al_0
See J.Tanaka Ph.D (2001) p136 for definition.
Definition: KFitBase.h:257
static void displayError(const char *file, const int line, const char *func, const enum ECode code)
Display a description of error and its location.
Definition: KFitError.h:72
ECode
ECode is a error code enumerate.
Definition: KFitError.h:34
@ kCannotGetARCSIN
Cannot get arcsin (bad track property or internal error)
Definition: KFitError.h:60
@ kCannotGetMatrixInverse
Cannot calculate matrix inverse (bad track property or internal error)
Definition: KFitError.h:58
@ kOutOfRange
Specified track-id out of range.
Definition: KFitError.h:42
@ kDivisionByZero
Division by zero (bad track property or internal error)
Definition: KFitError.h:56
@ kBadInitialCHIsq
Bad initial chi-square (internal error)
Definition: KFitError.h:53
@ kBadTrackSize
Track count too small to perform fit.
Definition: KFitError.h:47
KFitTrack is a container of the track information (Lorentz vector, position, and error matrix),...
Definition: KFitTrack.h:38
MakeMotherKFit is a class to build mother particle from kinematically fitted daughters.
enum KFitError::ECode setVertex(const HepPoint3D &v)
Set a vertex position of the mother particle.
enum KFitError::ECode addTrack(const KFitTrack &kp)
Add a track to the make-mother object.
enum KFitError::ECode doMake(void)
Perform a reconstruction of mother particle.
const CLHEP::HepSymMatrix getMotherError(void) const
Get an error matrix of the mother particle.
enum KFitError::ECode setCorrelation(const CLHEP::HepMatrix &e)
Set a correlation matrix.
const HepPoint3D getMotherPosition(void) const
Get a position of the mother particle.
enum KFitError::ECode setVertexError(const CLHEP::HepSymMatrix &e)
Set a vertex error matrix of the mother particle.
enum KFitError::ECode setTrackVertexError(const CLHEP::HepMatrix &e)
Set a vertex error matrix of the child particle in the addTrack'ed order.
const CLHEP::HepLorentzVector getMotherMomentum(void) const
Get a Lorentz vector of the mother particle.
enum KFitError::ECode setMagneticField(const double mf)
Change a magnetic field from the default value KFitConst::kDefaultMagneticField.
enum KFitError::ECode setIpTubeProfile(const CLHEP::HepLorentzVector &p, const HepPoint3D &x, const CLHEP::HepSymMatrix &e, const double q)
Set a virtual IP-tube track for the vertex constraint fit.
enum KFitError::ECode doFit5(void)
Perform a fixed-vertex-position fit mainly for slow pion.
enum KFitError::ECode setKnownVertex(const bool flag=true)
Tell the object to perform a fit with vertex position fixed.
enum KFitError::ECode prepareInputMatrix(void) override
Build grand matrices for minimum search from input-track properties.
enum KFitError::ECode doFit4(void)
Perform a IP-ellipsoid and vertex-constraint fit.
enum KFitError::ECode calculateNDF(void) override
Calculate an NDF of the fit.
enum KFitError::ECode setInitialVertex(const HepPoint3D &v)
Set an initial vertex point for the vertex-vertex constraint fit.
double getCHIsq(void) const override
Get a chi-square of the fit.
KFitTrack m_TubeTrack
Entity of the virtual IP-tube track.
VertexFitKFit(void)
Construct an object with no argument.
enum KFitError::ECode deleteTube(void)
Delete the virtual tube track to m_Tracks just after the internal minimization call.
bool m_FlagTube
Flag if to perform IP-tube constraint fit.
bool m_FlagKnownVertex
Flag controlled by setKnownVertex().
enum KFitError::ECode updateMother(Particle *mother)
Update mother particle.
const CLHEP::HepSymMatrix getVertexError(void) const
Get a fitted vertex error matrix.
enum KFitError::ECode prepareInputSubMatrix(void) override
Build sub-matrices for minimum search from input-track properties.
CLHEP::HepSymMatrix m_BeamError
Error matrix modeling the IP ellipsoid.
enum KFitError::ECode doFit(void)
Perform a vertex-constraint fit.
double getTrackPartCHIsq(void) const
Get a sum of the chi-square associated to the input tracks.
enum KFitError::ECode doFit3(void)
Perform a standard vertex-constraint fit including IP-tube constraint.
~VertexFitKFit(void)
Destruct the object.
enum KFitError::ECode makeCoreMatrix(void) override
Build matrices using the kinematical constraint.
HepPoint3D m_AfterVertex
Vertex position after the fit.
std::vector< CLHEP::HepMatrix > m_AfterTrackVertexError
Array of vertex error matrices after the fit.
double m_EachCHIsq[KFitConst::kMaxTrackCount2]
Container of chi-square's of the input tracks.
double getTrackCHIsq(const int id) const override
Get a chi-square of the track.
enum KFitError::ECode setCorrelationMode(const bool m)
Tell the object to perform a fit with track correlations.
int getTrackPartNDF(void) const
Get an NDF relevant to the getTrackPartCHIsq().
bool m_FlagBeam
Flag if to perform IP-ellipsoid constraint fit.
int m_iTrackTube
ID of the virtual tube track in the m_Tracks.
enum KFitError::ECode setIpProfile(const HepPoint3D &ip, const CLHEP::HepSymMatrix &ipe)
Set an IP-ellipsoid shape for the vertex constraint fit.
const HepPoint3D getVertex(const int flag=KFitConst::kAfterFit) const
Get a vertex position.
double m_CHIsqVertex
chi-square of the fit excluding IP-constraint part.
double getCHIsqVertex(void) const
Get a chi-square of the fit excluding IP-constraint part.
enum KFitError::ECode appendTube(void)
Add the virtual tube track to m_Tracks just before the internal minimization call.
CLHEP::HepSymMatrix m_AfterVertexError
Vertex error matrix after the fit.
bool m_CorrelationMode
Flag controlled by setCorrelationMode().
enum KFitError::ECode prepareOutputMatrix(void) override
Build an output error matrix.
const CLHEP::HepMatrix getTrackVertexError(const int id) const
Get a vertex error matrix of the track.
HepPoint3D m_BeforeVertex
Vertex position before the fit.
Abstract base class for different kinds of events.
Definition: ClusterUtils.h:24
STL namespace.
static constexpr double kInitialCHIsq
Initial chi-square value (internal use)
Definition: KFitConst.h:48
static const int kMaxTrackCount
Maximum track size.
Definition: KFitConst.h:40
static const int kMaxTrackCount2
Maximum track size (internal use)
Definition: KFitConst.h:42
static const int kNumber6
Constant 6 to check matrix size (internal use)
Definition: KFitConst.h:30
static const int kMaxIterationCount
Maximum iteration step (internal use)
Definition: KFitConst.h:45
static const int kAfterFit
Input parameter to specify after-fit when setting/getting a track attribute.
Definition: KFitConst.h:37
static const int kBeforeFit
Input parameter to specify before-fit when setting/getting a track attribute.
Definition: KFitConst.h:35
static const int kNumber7
Constant 7 to check matrix size (internal use)
Definition: KFitConst.h:32