12 #include <framework/logging/Logger.h>
14 #include <analysis/dataobjects/Particle.h>
15 #include <mdst/dataobjects/KLMCluster.h>
17 #include <analysis/VertexFitting/TreeFitter/RecoKlong.h>
18 #include <analysis/VertexFitting/TreeFitter/FitParams.h>
19 #include <analysis/VertexFitting/TreeFitter/ErrCode.h>
21 namespace TreeFitter {
24 RecoParticle(particle, mother),
38 Eigen::Matrix<double, 1, 3> vertexToCluster = Eigen::Matrix<double, 1, 3>::Zero(1, 3);
39 for (
unsigned int i = 0; i < 3; ++i) {
43 const double distanceToMother = vertexToCluster.norm();
45 const double energy2 = energy * energy;
47 const double absMom = -1 * std::sqrt(energy2 - pdgMass2);
51 for (
unsigned int i = 0; i < 3; ++i) {
53 fitparams.
getStateVector()(momindex + i) = absMom * vertexToCluster(i) / distanceToMother;
58 return ErrCode(ErrCode::Status::success);
63 return ErrCode(ErrCode::Status::success);
72 const double factorX = 1000;
74 fitparams.getCovariance().block<4, 4>(momindex, momindex) =
75 Eigen::Matrix<double, 4, 4>::Identity(4, 4) * factorE;
77 fitparams.getCovariance().block<3, 3>(posindex, posindex) =
78 Eigen::Matrix<double, 3, 3>::Identity(3, 3) * factorX;
80 return ErrCode(ErrCode::Status::success);
87 const TVector3 cluster_pos = cluster->getClusterPosition();
92 TMatrixDSym cov7 = cluster->getError7x7();
94 for (
int row = 0; row < 3; ++row) {
95 for (
int col = 0; col < 3; ++col) {
109 cluster->getMomentumMag());
113 if ((std::abs(p_vec(0)) >= std::abs(p_vec(1))) && (std::abs(p_vec(0)) >= std::abs(p_vec(2)))) {
115 }
else if ((std::abs(p_vec(1)) >= std::abs(p_vec(0))) && (std::abs(p_vec(1)) >= std::abs(p_vec(2)))) {
117 }
else if ((std::abs(p_vec(2)) >= std::abs(p_vec(1))) && (std::abs(p_vec(2)) >= std::abs(p_vec(0)))) {
120 B2ERROR(
"Could not estimate highest momentum for Klong constraint. Aborting this fit.\n px: "
121 << p_vec(0) <<
" py: " << p_vec(1) <<
" pz: " << p_vec(2) <<
" calculated from Ec: " <<
m_clusterPars(3));
122 return ErrCode(ErrCode::Status::photondimerror);
125 return ErrCode(ErrCode::Status::success);
134 const Eigen::Matrix<double, 1, 3> x_vertex = fitparams.getStateVector().segment(posindex, 3);
135 const Eigen::Matrix<double, 1, 3> p_vec = fitparams.getStateVector().segment(momindex, 3);
137 if (0 == p_vec[
m_i1]) {
return ErrCode(ErrCode::Status::klongdimerror); }
141 const double mom = p_vec.norm();
142 const double energy = fitparams.getStateVector()(momindex + 3);
145 Eigen::Matrix<double, 3, 1> residual3 = Eigen::Matrix<double, 3, 1>::Zero(3, 1);
150 Eigen::Matrix<double, 3, 4> P = Eigen::Matrix<double, 3, 4>::Zero(3, 4);
160 p.getResiduals().segment(0, 3) = residual3;
162 p.getV() = P *
m_covariance.selfadjointView<Eigen::Lower>() * P.transpose();
166 p.getH()(0, posindex +
m_i1) = p_vec[
m_i2] / p_vec[
m_i1];
167 p.getH()(0, posindex +
m_i2) = -1.0;
168 p.getH()(0, posindex +
m_i3) = 0;
170 p.getH()(1, posindex +
m_i1) = p_vec[
m_i3] / p_vec[
m_i1];
171 p.getH()(1, posindex +
m_i2) = 0;
172 p.getH()(1, posindex +
m_i3) = -1.0;
174 p.getH()(0, momindex +
m_i1) = p_vec[
m_i2] * elim / p_vec[
m_i1];
175 p.getH()(0, momindex +
m_i2) = -1. * elim;
176 p.getH()(0, momindex +
m_i3) = 0;
178 p.getH()(1, momindex +
m_i1) = p_vec[
m_i3] * elim / p_vec[
m_i1];;
179 p.getH()(1, momindex +
m_i2) = 0;
180 p.getH()(1, momindex +
m_i3) = -1. * elim;
182 p.getH()(2, momindex +
m_i1) = -1. * p_vec[
m_i1] / mom;
183 p.getH()(2, momindex +
m_i2) = -1. * p_vec[
m_i2] / mom;
184 p.getH()(2, momindex +
m_i3) = -1. * p_vec[
m_i3] / mom;
185 p.getH()(2, momindex + 3) = -1;
187 return ErrCode(ErrCode::Status::success);