12 #include <framework/logging/Logger.h>
14 #include <analysis/dataobjects/Particle.h>
15 #include <mdst/dataobjects/ECLCluster.h>
17 #include <analysis/ClusterUtility/ClusterUtils.h>
19 #include <analysis/VertexFitting/TreeFitter/RecoPhoton.h>
20 #include <analysis/VertexFitting/TreeFitter/FitParams.h>
21 #include <analysis/VertexFitting/TreeFitter/ErrCode.h>
23 #include <framework/gearbox/Const.h>
25 namespace TreeFitter {
28 RecoParticle(particle, mother),
31 m_useEnergy(useEnergy(*particle)),
42 Eigen::Matrix<double, 1, 3> vertexToCluster = Eigen::Matrix<double, 1, 3>::Zero(1, 3);
43 for (
unsigned int i = 0; i < 3; ++i) {
47 const double distanceToMother = vertexToCluster.norm();
51 for (
unsigned int i = 0; i < 3; ++i) {
53 fitparams.
getStateVector()(momindex + i) = energy * vertexToCluster(i) / distanceToMother;
56 return ErrCode(ErrCode::Status::success);
61 return ErrCode(ErrCode::Status::success);
82 const double factorX = 1000;
85 Eigen::Matrix<double, 4, 4>::Identity(4, 4) * factorE;
88 Eigen::Matrix<double, 3, 3>::Identity(3, 3) * factorX;
90 return ErrCode(ErrCode::Status::success);
97 const TVector3 centroid = cluster->getClusterPosition();
98 const double energy = cluster->getEnergy(clusterhypo);
104 TMatrixDSym cov_EPhiTheta = cluster->getCovarianceMatrix3x3();
106 Eigen::Matrix<double, 2, 2> covPhiTheta = Eigen::Matrix<double, 2, 2>::Zero(2, 2);
108 for (
int row = 0; row < 2; ++row) {
109 for (
int col = 0; col < 2; ++col) {
110 covPhiTheta(row, col) = cov_EPhiTheta[row + 1][col + 1];
115 const double R = cluster->getR();
116 const double theta = cluster->getPhi();
117 const double phi = cluster->getTheta();
119 const double st = std::sin(theta);
120 const double ct = std::cos(theta);
121 const double sp = std::sin(phi);
122 const double cp = std::cos(phi);
124 Eigen::Matrix<double, 2, 3> polarToCartesian = Eigen::Matrix<double, 2, 3>::Zero(2, 3);
127 polarToCartesian(0, 0) = -1. * R * st * sp;
128 polarToCartesian(0, 1) = R * st * cp;
129 polarToCartesian(0, 2) = 0 ;
131 polarToCartesian(1, 0) = R * ct * cp;
132 polarToCartesian(1, 1) = R * ct * sp;
133 polarToCartesian(1, 2) = -1. * R * st ;
135 m_covariance.block<3, 3>(0, 0) = polarToCartesian.transpose() * covPhiTheta * polarToCartesian;
146 if ((std::abs(p_vec(0)) >= std::abs(p_vec(1))) && (std::abs(p_vec(0)) >= std::abs(p_vec(2)))) {
148 }
else if ((std::abs(p_vec(1)) >= std::abs(p_vec(0))) && (std::abs(p_vec(1)) >= std::abs(p_vec(2)))) {
150 }
else if ((std::abs(p_vec(2)) >= std::abs(p_vec(1))) && (std::abs(p_vec(2)) >= std::abs(p_vec(0)))) {
153 B2ERROR(
"Could not estimate highest momentum for photon constraint. Aborting this fit.\n px: "
154 << p_vec(0) <<
" py: " << p_vec(1) <<
" pz: " << p_vec(2) <<
" calculated from Ec: " <<
m_clusterPars(3));
155 return ErrCode(ErrCode::Status::photondimerror);
159 return ErrCode(ErrCode::Status::success);
183 const Eigen::Matrix<double, 1, 3> x_vertex = fitparams.getStateVector().segment(posindex, 3);
184 const Eigen::Matrix<double, 1, 3> p_vec = fitparams.getStateVector().segment(momindex, 3);
186 if (0 == p_vec[
m_i1]) {
return ErrCode(ErrCode::photondimerror); }
190 const double mom = p_vec.norm();
193 Eigen::Matrix<double, 3, 1> residual3 = Eigen::Matrix<double, 3, 1>::Zero(3, 1);
199 Eigen::Matrix<double, 3, 4> P = Eigen::Matrix<double, 3, 4>::Zero(3, 4);
208 p.getResiduals().segment(0, 3) = residual3;
210 p.getV() = P *
m_covariance.selfadjointView<Eigen::Lower>() * P.transpose();
214 p.getH()(0, posindex +
m_i1) = p_vec[
m_i2] / p_vec[
m_i1];
215 p.getH()(0, posindex +
m_i2) = -1.0;
216 p.getH()(0, posindex +
m_i3) = 0;
218 p.getH()(1, posindex +
m_i1) = p_vec[
m_i3] / p_vec[
m_i1];
219 p.getH()(1, posindex +
m_i2) = 0;
220 p.getH()(1, posindex +
m_i3) = -1.0;
223 p.getH()(0, momindex +
m_i1) = p_vec[
m_i2] * elim / p_vec[
m_i1];
224 p.getH()(0, momindex +
m_i2) = -1. * elim;
225 p.getH()(0, momindex +
m_i3) = 0;
227 p.getH()(1, momindex +
m_i1) = p_vec[
m_i3] * elim / p_vec[
m_i1];
228 p.getH()(1, momindex +
m_i2) = 0;
229 p.getH()(1, momindex +
m_i3) = -1. * elim;
231 p.getH()(2, momindex +
m_i1) = -1. * p_vec[
m_i1] / mom;
232 p.getH()(2, momindex +
m_i2) = -1. * p_vec[
m_i2] / mom;
233 p.getH()(2, momindex +
m_i3) = -1. * p_vec[
m_i3] / mom;
237 return ErrCode(ErrCode::Status::success);