11 #include <analysis/VertexFitting/TreeFitter/RecoTrack.h>
12 #include <analysis/VertexFitting/TreeFitter/FitParams.h>
13 #include <analysis/VertexFitting/TreeFitter/HelixUtils.h>
15 #include <framework/gearbox/Const.h>
16 #include <mdst/dataobjects/Track.h>
20 namespace TreeFitter {
21 constexpr
double pi = TMath::Pi();
22 constexpr
double twoPi = TMath::TwoPi();
25 RecoParticle(particle, mother),
27 m_trackfit(particle->getTrackFitResult()),
34 m_covariance = Eigen::Matrix<double, 5, 5>::Zero(5, 5);
48 return ErrCode(ErrCode::Status::success);
53 return ErrCode(ErrCode::Status::success);
62 for (
int row = 0; row < 3; ++row) {
63 fitparams.getCovariance()(momindex + row, momindex + row) = 1000 * p4Err[row][row];
66 return ErrCode(ErrCode::Status::success);
74 fitparams.getStateVector()(posindexmother),
75 fitparams.getStateVector()(posindexmother + 1));
76 return ErrCode(ErrCode::Status::success);
83 for (
unsigned int i = 0; i < trackParameter.size(); ++i) {
87 for (
int row = 0; row < 5; ++row) {
88 for (
int col = 0; col <= row; ++col) {
94 return ErrCode(ErrCode::Status::success);
103 Eigen::Matrix<double, 1, 6> positionAndMom = Eigen::Matrix<double, 1, 6>::Zero(1, 6);
104 positionAndMom.segment(0, 3) = fitparams.getStateVector().segment(posindexmother, 3);
105 positionAndMom.segment(3, 3) = fitparams.getStateVector().segment(momindex, 3);
106 Eigen::Matrix<double, 5, 6> jacobian = Eigen::Matrix<double, 5, 6>::Zero(5, 6);
132 auto* nonconst =
const_cast<RecoTrack*
>(
this);
134 nonconst->updateParams(
m_flt);
137 Eigen::Matrix<double, 1, 5> helixpars(5);
138 helixpars(0) = helix.getD0();
139 helixpars(1) = helix.getPhi0();
140 helixpars(2) = helix.getOmega();
141 helixpars(3) = helix.getZ0();
142 helixpars(4) = helix.getTanLambda();
144 p.getResiduals().segment(0, 5) =
m_params - helixpars;
147 double phiResidual = p.getResiduals().segment(0, 5)(1);
148 phiResidual = std::fmod(phiResidual + pi, twoPi);
149 if (phiResidual < 0) phiResidual += twoPi;
151 p.getResiduals().segment(0, 5)(1) = phiResidual;
153 p.getV().triangularView<Eigen::Lower>() =
m_covariance.triangularView<Eigen::Lower>();
155 p.getH().block<5, 3>(0, posindexmother) = -1.0 * jacobian.block<5, 3>(0, 0);
156 p.getH().block<5, 3>(0, momindex) = -1.0 * jacobian.block<5, 3>(0, 3);