14 #include <TMatrixFSym.h>
16 #include <analysis/VertexFitting/KFit/FourCFitKFit.h>
17 #include <analysis/VertexFitting/KFit/MakeMotherKFit.h>
18 #include <analysis/utility/CLHEPToROOT.h>
19 #include <TLorentzVector.h>
24 using namespace Belle2::analysis;
25 using namespace CLHEP;
27 FourCFitKFit::FourCFitKFit()
30 m_FlagTrackVertexError =
false;
31 m_FlagFitIncludingVertex =
false;
32 m_FlagAtDecayPoint =
true;
33 m_NecessaryTrackCount = 2;
34 m_d = HepMatrix(4, 1, 0);
35 m_V_D = HepMatrix(4, 4, 0);
36 m_lam = HepMatrix(4, 1, 0);
37 m_AfterVertexError = HepSymMatrix(3, 0);
38 m_InvariantMass = -1.0;
39 m_FourMomentum = TLorentzVector();
43 FourCFitKFit::~FourCFitKFit() =
default;
50 return m_ErrorCode = KFitError::kNoError;
55 FourCFitKFit::setVertexError(
const HepSymMatrix& e) {
58 m_ErrorCode = KFitError::kBadMatrixSize;
59 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
63 m_BeforeVertexError = e;
64 m_FlagFitIncludingVertex =
true;
66 return m_ErrorCode = KFitError::kNoError;
71 FourCFitKFit::setInvariantMass(
const double m) {
74 return m_ErrorCode = KFitError::kNoError;
79 FourCFitKFit::setFourMomentum(
const TLorentzVector& m) {
82 return m_ErrorCode = KFitError::kNoError;
87 FourCFitKFit::setFlagAtDecayPoint(
const bool flag) {
88 m_FlagAtDecayPoint = flag;
90 return m_ErrorCode = KFitError::kNoError;
95 FourCFitKFit::fixMass() {
96 m_IsFixMass.push_back(
true);
98 return m_ErrorCode = KFitError::kNoError;
103 FourCFitKFit::unfixMass() {
104 m_IsFixMass.push_back(
false);
106 return m_ErrorCode = KFitError::kNoError;
111 FourCFitKFit::setTrackVertexError(
const HepMatrix& e) {
112 if (e.num_row() != 3 || e.num_col() != KFitConst::kNumber7)
114 m_ErrorCode = KFitError::kBadMatrixSize;
115 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
119 m_BeforeTrackVertexError.push_back(e);
120 m_FlagTrackVertexError =
true;
121 m_FlagFitIncludingVertex =
true;
123 return m_ErrorCode = KFitError::kNoError;
128 FourCFitKFit::setTrackZeroVertexError() {
129 HepMatrix zero(3, KFitConst::kNumber7, 0);
131 return this->setTrackVertexError(zero);
136 FourCFitKFit::setCorrelation(
const HepMatrix& m) {
137 return KFitBase::setCorrelation(m);
142 FourCFitKFit::setZeroCorrelation() {
143 return KFitBase::setZeroCorrelation();
148 FourCFitKFit::getVertex(
const int flag)
const
150 if (flag == KFitConst::kAfterFit && !isFitted())
return HepPoint3D();
153 case KFitConst::kBeforeFit:
154 return m_BeforeVertex;
156 case KFitConst::kAfterFit:
157 return m_AfterVertex;
160 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kOutOfRange);
167 FourCFitKFit::getVertexError(
const int flag)
const
169 if (flag == KFitConst::kAfterFit && !isFitted())
return HepSymMatrix(3, 0);
171 if (flag == KFitConst::kBeforeFit)
172 return m_BeforeVertexError;
173 else if (flag == KFitConst::kAfterFit && m_FlagFitIncludingVertex)
174 return m_AfterVertexError;
176 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kOutOfRange);
177 return HepSymMatrix(3, 0);
183 FourCFitKFit::getInvariantMass()
const
185 return m_InvariantMass;
190 FourCFitKFit::getFlagAtDecayPoint()
const
192 return m_FlagAtDecayPoint;
197 FourCFitKFit::getFlagFitWithVertex()
const
199 return m_FlagFitIncludingVertex;
204 FourCFitKFit::getCHIsq()
const
211 FourCFitKFit::getTrackVertexError(
const int id,
const int flag)
const
213 if (flag == KFitConst::kAfterFit && !isFitted())
return HepMatrix(3, KFitConst::kNumber7, 0);
214 if (!isTrackIDInRange(
id))
return HepMatrix(3, KFitConst::kNumber7, 0);
216 if (flag == KFitConst::kBeforeFit)
217 return m_BeforeTrackVertexError[id];
218 else if (flag == KFitConst::kAfterFit && m_FlagFitIncludingVertex)
219 return m_AfterTrackVertexError[id];
221 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kOutOfRange);
222 return HepMatrix(3, KFitConst::kNumber7, 0);
228 FourCFitKFit::getTrackCHIsq(
const int id)
const
230 if (!isFitted())
return -1;
231 if (!isTrackIDInRange(
id))
return -1;
233 if (m_IsFixMass[
id]) {
235 HepMatrix da(m_Tracks[
id].getFitParameter(KFitConst::kBeforeFit) - m_Tracks[
id].getFitParameter(KFitConst::kAfterFit));
237 const double chisq = (da.T() * (m_Tracks[id].getFitError(KFitConst::kBeforeFit).inverse(err_inverse)) * da)[0][0];
240 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kCannotGetMatrixInverse);
248 HepMatrix da(m_Tracks[
id].getMomPos(KFitConst::kBeforeFit) - m_Tracks[
id].getMomPos(KFitConst::kAfterFit));
250 const double chisq = (da.T() * (m_Tracks[id].getError(KFitConst::kBeforeFit).inverse(err_inverse)) * da)[0][0];
253 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kCannotGetMatrixInverse);
264 FourCFitKFit::getCorrelation(
const int id1,
const int id2,
const int flag)
const
266 if (flag == KFitConst::kAfterFit && !isFitted())
return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
267 if (!isTrackIDInRange(id1))
return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
268 if (!isTrackIDInRange(id2))
return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
271 case KFitConst::kBeforeFit:
272 return KFitBase::getCorrelation(id1, id2, flag);
274 case KFitConst::kAfterFit:
276 this->getTrackMomentum(id1),
277 this->getTrackMomentum(id2),
278 m_V_al_1.sub(KFitConst::kNumber7 * id1 + 1, KFitConst::kNumber7 * (id1 + 1), KFitConst::kNumber7 * id2 + 1,
279 KFitConst::kNumber7 * (id2 + 1)),
284 KFitError::displayError(__FILE__, __LINE__, __func__, KFitError::kOutOfRange);
285 return HepMatrix(KFitConst::kNumber7, KFitConst::kNumber7, 0);
291 FourCFitKFit::doFit() {
292 return KFitBase::doFit1();
297 FourCFitKFit::prepareInputMatrix() {
298 if (m_TrackCount > KFitConst::kMaxTrackCount)
300 m_ErrorCode = KFitError::kBadTrackSize;
301 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
306 if (m_IsFixMass.size() == 0)
310 for (
int i = 0; i < m_TrackCount; i++) this->fixMass();
311 }
else if (m_IsFixMass.size() != (
unsigned int)m_TrackCount)
313 m_ErrorCode = KFitError::kBadTrackSize;
314 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
319 if (!m_FlagFitIncludingVertex)
322 m_al_0 = HepMatrix(KFitConst::kNumber7 * m_TrackCount, 1, 0);
323 m_property = HepMatrix(m_TrackCount, 3, 0);
324 m_V_al_0 = HepSymMatrix(KFitConst::kNumber7 * m_TrackCount, 0);
326 for (
auto& track : m_Tracks) {
328 m_al_0[index * KFitConst::kNumber7 + 0][0] = track.getMomentum(KFitConst::kBeforeFit).x();
329 m_al_0[index * KFitConst::kNumber7 + 1][0] = track.getMomentum(KFitConst::kBeforeFit).y();
330 m_al_0[index * KFitConst::kNumber7 + 2][0] = track.getMomentum(KFitConst::kBeforeFit).z();
331 m_al_0[index * KFitConst::kNumber7 + 3][0] = track.getMomentum(KFitConst::kBeforeFit).t();
332 m_al_0[index * KFitConst::kNumber7 + 4][0] = track.getPosition(KFitConst::kBeforeFit).x();
333 m_al_0[index * KFitConst::kNumber7 + 5][0] = track.getPosition(KFitConst::kBeforeFit).y();
334 m_al_0[index * KFitConst::kNumber7 + 6][0] = track.getPosition(KFitConst::kBeforeFit).z();
336 m_V_al_0.sub(index * KFitConst::kNumber7 + 1, track.getError(KFitConst::kBeforeFit));
338 m_property[index][0] = track.getCharge();
339 m_property[index][1] = track.getMass();
340 const double c = KFitConst::kLightSpeed;
342 m_property[index][2] = -c * m_MagneticField * track.getCharge();
347 if (m_FlagCorrelation) {
348 this->prepareCorrelation();
349 if (m_ErrorCode != KFitError::kNoError) {
350 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
359 m_V_al_1 = HepMatrix(KFitConst::kNumber7 * m_TrackCount, KFitConst::kNumber7 * m_TrackCount, 0);
360 m_D = m_V_al_1.sub(1, 4, 1, KFitConst::kNumber7 * m_TrackCount);
365 m_al_0 = HepMatrix(KFitConst::kNumber7 * m_TrackCount + 3, 1, 0);
366 m_property = HepMatrix(m_TrackCount, 3, 0);
367 m_V_al_0 = HepSymMatrix(KFitConst::kNumber7 * m_TrackCount + 3, 0);
369 for (
auto& track : m_Tracks)
372 m_al_0[index * KFitConst::kNumber7 + 0][0] = track.getMomentum(KFitConst::kBeforeFit).x();
373 m_al_0[index * KFitConst::kNumber7 + 1][0] = track.getMomentum(KFitConst::kBeforeFit).y();
374 m_al_0[index * KFitConst::kNumber7 + 2][0] = track.getMomentum(KFitConst::kBeforeFit).z();
375 m_al_0[index * KFitConst::kNumber7 + 3][0] = track.getMomentum(KFitConst::kBeforeFit).t();
376 m_al_0[index * KFitConst::kNumber7 + 4][0] = track.getPosition(KFitConst::kBeforeFit).x();
377 m_al_0[index * KFitConst::kNumber7 + 5][0] = track.getPosition(KFitConst::kBeforeFit).y();
378 m_al_0[index * KFitConst::kNumber7 + 6][0] = track.getPosition(KFitConst::kBeforeFit).z();
380 m_V_al_0.sub(index * KFitConst::kNumber7 + 1, track.getError(KFitConst::kBeforeFit));
382 m_property[index][0] = track.getCharge();
383 m_property[index][1] = track.getMass();
384 const double c = KFitConst::kLightSpeed;
386 m_property[index][2] = -c * m_MagneticField * track.getCharge();
391 m_al_0[KFitConst::kNumber7 * m_TrackCount + 0][0] = m_BeforeVertex.x();
392 m_al_0[KFitConst::kNumber7 * m_TrackCount + 1][0] = m_BeforeVertex.y();
393 m_al_0[KFitConst::kNumber7 * m_TrackCount + 2][0] = m_BeforeVertex.z();
394 m_V_al_0.sub(KFitConst::kNumber7 * m_TrackCount + 1, m_BeforeVertexError);
397 if (m_FlagCorrelation)
399 this->prepareCorrelation();
400 if (m_ErrorCode != KFitError::kNoError) {
401 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
410 m_V_al_1 = HepMatrix(KFitConst::kNumber7 * m_TrackCount + 3, KFitConst::kNumber7 * m_TrackCount + 3, 0);
411 m_D = m_V_al_1.sub(1, 4, 1, KFitConst::kNumber7 * m_TrackCount + 3);
414 return m_ErrorCode = KFitError::kNoError;
419 FourCFitKFit::prepareInputSubMatrix() {
421 sprintf(buf,
"%s:%s(): internal error; this function should never be called", __FILE__, __func__);
425 return KFitError::kOutOfRange;
430 FourCFitKFit::prepareCorrelation() {
431 if (m_BeforeCorrelation.size() !=
static_cast<unsigned int>(m_TrackCount * (m_TrackCount - 1) / 2))
433 m_ErrorCode = KFitError::kBadCorrelationSize;
434 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
438 int row = 0, col = 0;
440 for (
auto& hm : m_BeforeCorrelation)
444 if (row == m_TrackCount) {
450 for (
int i = KFitConst::kNumber7 * row; i < KFitConst::kNumber7 * (row + 1); i++) {
451 for (
int j = KFitConst::kNumber7 * col; j < KFitConst::kNumber7 * (col + 1); j++) {
452 m_V_al_0[i][j] = hm[ii][jj];
460 if (m_FlagFitIncludingVertex)
463 m_V_al_0.sub(KFitConst::kNumber7 * m_TrackCount + 1, m_BeforeVertexError);
466 if (m_FlagTrackVertexError) {
467 if (m_BeforeTrackVertexError.size() != (
unsigned int)m_TrackCount) {
468 m_ErrorCode = KFitError::kBadCorrelationSize;
469 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
474 for (
auto& hm : m_BeforeTrackVertexError) {
475 for (
int j = 0; j < 3; j++)
for (
int k = 0; k < KFitConst::kNumber7; k++) {
476 m_V_al_0[j + KFitConst::kNumber7 * m_TrackCount][k + i * KFitConst::kNumber7] = hm[j][k];
483 return m_ErrorCode = KFitError::kNoError;
488 FourCFitKFit::prepareOutputMatrix() {
491 for (
auto& pdata : m_Tracks)
495 h3v.setX(m_al_1[index * KFitConst::kNumber7 + 0][0]);
496 h3v.setY(m_al_1[index * KFitConst::kNumber7 + 1][0]);
497 h3v.setZ(m_al_1[index * KFitConst::kNumber7 + 2][0]);
498 pdata.setMomentum(HepLorentzVector(h3v, m_al_1[index * KFitConst::kNumber7 + 3][0]), KFitConst::kAfterFit);
501 m_al_1[index * KFitConst::kNumber7 + 4][0],
502 m_al_1[index * KFitConst::kNumber7 + 5][0],
503 m_al_1[index * KFitConst::kNumber7 + 6][0]), KFitConst::kAfterFit);
505 pdata.setError(this->makeError3(pdata.getMomentum(),
507 index * KFitConst::kNumber7 + 1,
508 (index + 1)*KFitConst::kNumber7,
509 index * KFitConst::kNumber7 + 1,
510 (index + 1)*KFitConst::kNumber7), m_IsFixMass[index]),
511 KFitConst::kAfterFit);
512 if (m_ErrorCode != KFitError::kNoError)
break;
516 if (m_FlagFitIncludingVertex)
519 m_AfterVertex.setX(m_al_1[KFitConst::kNumber7 * m_TrackCount + 0][0]);
520 m_AfterVertex.setY(m_al_1[KFitConst::kNumber7 * m_TrackCount + 1][0]);
521 m_AfterVertex.setZ(m_al_1[KFitConst::kNumber7 * m_TrackCount + 2][0]);
523 for (
int i = 0; i < 3; i++)
for (
int j = i; j < 3; j++) {
524 m_AfterVertexError[i][j] = m_V_al_1[KFitConst::kNumber7 * m_TrackCount + i][KFitConst::kNumber7 * m_TrackCount + j];
527 for (
int i = 0; i < m_TrackCount; i++) {
528 HepMatrix hm(3, KFitConst::kNumber7, 0);
529 for (
int j = 0; j < 3; j++)
for (
int k = 0; k < KFitConst::kNumber7; k++) {
530 hm[j][k] = m_V_al_1[KFitConst::kNumber7 * m_TrackCount + j][KFitConst::kNumber7 * i + k];
533 m_AfterTrackVertexError.push_back(this->makeError4(m_Tracks[i].getMomentum(), hm));
535 m_AfterTrackVertexError.push_back(hm);
539 m_AfterVertex = m_BeforeVertex;
542 return m_ErrorCode = KFitError::kNoError;
547 FourCFitKFit::makeCoreMatrix() {
548 if (!m_FlagFitIncludingVertex)
551 HepMatrix al_1_prime(m_al_1);
552 HepMatrix Sum_al_1(4, 1, 0);
553 double energy[KFitConst::kMaxTrackCount2];
555 for (
int i = 0; i < m_TrackCount; i++) {
556 energy[i] = sqrt(al_1_prime[i * KFitConst::kNumber7 + 0][0] * al_1_prime[i * KFitConst::kNumber7 + 0][0] +
557 al_1_prime[i * KFitConst::kNumber7 + 1][0] * al_1_prime[i * KFitConst::kNumber7 + 1][0] +
558 al_1_prime[i * KFitConst::kNumber7 + 2][0] * al_1_prime[i * KFitConst::kNumber7 + 2][0] +
559 m_property[i][1] * m_property[i][1]);
562 for (
int i = 0; i < m_TrackCount; i++) {
564 for (
int j = 0; j < 4; j++) Sum_al_1[j][0] += al_1_prime[i * KFitConst::kNumber7 + j][0];
567 m_d[0][0] = Sum_al_1[0][0] - m_FourMomentum.Px();
568 m_d[1][0] = Sum_al_1[1][0] - m_FourMomentum.Py();
569 m_d[2][0] = Sum_al_1[2][0] - m_FourMomentum.Pz();
570 m_d[3][0] = Sum_al_1[3][0] - m_FourMomentum.E();
572 for (
int i = 0; i < m_TrackCount; i++) {
573 if (energy[i] == 0) {
574 m_ErrorCode = KFitError::kDivisionByZero;
575 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
579 for (
int l = 0; l < 4; l++) {
580 for (
int n = 0; n < 6; n++) {
581 if (l == n) m_D[l][i * KFitConst::kNumber7 + n] = 1;
582 else m_D[l][i * KFitConst::kNumber7 + n] = 0;
590 HepMatrix al_1_prime(m_al_1);
591 HepMatrix Sum_al_1(7, 1, 0);
592 double energy[KFitConst::kMaxTrackCount2];
594 for (
int i = 0; i < m_TrackCount; i++)
596 const double a = m_property[i][2];
597 al_1_prime[i * KFitConst::kNumber7 + 0][0] -= a * (al_1_prime[KFitConst::kNumber7 * m_TrackCount + 1][0] - al_1_prime[i *
598 KFitConst::kNumber7 + 5][0]);
599 al_1_prime[i * KFitConst::kNumber7 + 1][0] += a * (al_1_prime[KFitConst::kNumber7 * m_TrackCount + 0][0] - al_1_prime[i *
600 KFitConst::kNumber7 + 4][0]);
601 energy[i] = sqrt(al_1_prime[i * KFitConst::kNumber7 + 0][0] * al_1_prime[i * KFitConst::kNumber7 + 0][0] +
602 al_1_prime[i * KFitConst::kNumber7 + 1][0] * al_1_prime[i * KFitConst::kNumber7 + 1][0] +
603 al_1_prime[i * KFitConst::kNumber7 + 2][0] * al_1_prime[i * KFitConst::kNumber7 + 2][0] +
604 m_property[i][1] * m_property[i][1]);
605 Sum_al_1[6][0] = + a;
608 for (
int i = 0; i < m_TrackCount; i++)
610 if (energy[i] == 0) {
611 m_ErrorCode = KFitError::kDivisionByZero;
612 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
616 if (m_IsFixMass[i]) {
617 double invE = 1. / energy[i];
618 Sum_al_1[3][0] += energy[i];
619 Sum_al_1[4][0] += al_1_prime[i * KFitConst::kNumber7 + 1][0] * m_property[i][2] * invE;
620 Sum_al_1[5][0] += al_1_prime[i * KFitConst::kNumber7 + 0][0] * m_property[i][2] * invE;
622 Sum_al_1[3][0] += al_1_prime[i * KFitConst::kNumber7 + 3][0];
625 for (
int j = 0; j < 3; j++) Sum_al_1[j][0] += al_1_prime[i * KFitConst::kNumber7 + j][0];
628 m_d[0][0] = Sum_al_1[0][0] - m_FourMomentum.Px();
629 m_d[1][0] = Sum_al_1[1][0] - m_FourMomentum.Py();
630 m_d[2][0] = Sum_al_1[2][0] - m_FourMomentum.Pz();
631 m_d[3][0] = Sum_al_1[3][0] - m_FourMomentum.E();
633 for (
int i = 0; i < m_TrackCount; i++)
635 if (energy[i] == 0) {
636 m_ErrorCode = KFitError::kDivisionByZero;
637 KFitError::displayError(__FILE__, __LINE__, __func__, m_ErrorCode);
641 for (
int l = 0; l < 4; l++) {
642 for (
int n = 0; n < 6; n++) {
643 if (l == n) m_D[l][i * KFitConst::kNumber7 + n] = 1;
644 else m_D[l][i * KFitConst::kNumber7 + n] = 0;
649 m_D[0][KFitConst::kNumber7 * m_TrackCount + 0] = 2.*(Sum_al_1[3][0] * Sum_al_1[4][0] - Sum_al_1[1][0] * Sum_al_1[6][0]);
650 m_D[0][KFitConst::kNumber7 * m_TrackCount + 1] = -2.*(Sum_al_1[3][0] * Sum_al_1[5][0] - Sum_al_1[0][0] * Sum_al_1[6][0]);
651 m_D[0][KFitConst::kNumber7 * m_TrackCount + 2] = 0.;
654 return m_ErrorCode = KFitError::kNoError;
659 FourCFitKFit::calculateNDF() {
662 return m_ErrorCode = KFitError::kNoError;
669 unsigned n = getTrackCount();
670 for (
unsigned i = 0; i < n; ++i) {
671 kmm.
addTrack(getTrackMomentum(i), getTrackPosition(i), getTrackError(i),
672 getTrack(i).getCharge());
673 if (getFlagFitWithVertex())
675 for (
unsigned j = i + 1; j < n; ++j) {
680 if (getFlagFitWithVertex())
682 m_ErrorCode = kmm.
doMake();
683 if (m_ErrorCode != KFitError::kNoError)
685 double chi2 = getCHIsq();
687 double prob = TMath::Prob(chi2, ndf);
689 bool haschi2 = mother->hasExtraInfo(
"chiSquared");
691 mother->setExtraInfo(
"chiSquared", chi2);
692 mother->setExtraInfo(
"ndf", ndf);
694 mother->addExtraInfo(
"chiSquared", chi2);
695 mother->addExtraInfo(
"ndf", ndf);
698 mother->updateMomentum(
703 m_ErrorCode = KFitError::kNoError;