Update method.
34 {
36 Trk::LinearizedTrack * linTrack = trk.linState();
37
38
39 if(linTrack == nullptr)
40 {
42 {
43
44 msg(MSG::INFO) <<
"The VxTrackAtVertex passed does not have a Linearized Track" <<
endmsg;
45 msg(MSG::INFO) <<
"Please produce one with corresponding LinearizedTrackFactory"<<
endmsg;
46 msg(MSG::INFO) <<
"The VxTrackAtVertex returned not refitted"<<
endmsg;
47
48 }else{
50 {
51 msg(MSG::DEBUG) <<
"The VxTrackAtVertex passed does not have a Linearized Track" <<
endmsg;
53 msg(MSG::DEBUG) <<
"The VxTrackAtVertex returned not refitted"<<
endmsg;
54 }
55 }
56
57 return;
58 }
59
60
61 const AmgMatrix(5,3) &
A = linTrack->positionJacobian();
62 const AmgMatrix(5,3) &
B = linTrack->momentumJacobian();
63 const AmgVector(5) & trkParameters = linTrack->expectedParametersAtPCA();
64
65
66
67 const AmgSymMatrix(5) & trkParametersWeight = linTrack->expectedWeightAtPCA();
68
69
70 AmgSymMatrix(3) Sm = B.transpose()*(trkParametersWeight*B);
71
72 if (Sm.determinant() == 0.0) {
73 ATH_MSG_WARNING(
"Matrix can not be inverted, new type of check as part of Eigen, please monitor."
74 <<
endmsg <<
"Matrix Sm = " << Sm);
75 return;
76 }
77 Sm = Sm.inverse().eval();
78 const AmgVector(5) & theResidual = linTrack->constantTerm();
79
80
81 Amg::Vector3D newTrackMomentum = Sm*
B.transpose() * trkParametersWeight
82 * (trkParameters - theResidual -
A*fit_vrt_pos);
83
84
85 AmgVector(5) refTrkPar; refTrkPar.setZero();
86 refTrkPar(Trk::
phi) = newTrackMomentum(0);
87 refTrkPar(Trk::
theta) = newTrackMomentum(1);
88 refTrkPar(Trk::
qOverP)= newTrackMomentum(2);
89
90
91
92
93
94
95
96
97
98
100 {
103 if(
msgLvl(MSG::DEBUG))
msg(MSG::DEBUG)<<
" out of range, now "
108 if(
msgLvl(MSG::DEBUG))
msg(MSG::DEBUG)<<
" out of range, now "
110 }
111
113 {
114 if(
msgLvl(MSG::DEBUG))
msg(MSG::DEBUG)<<
" Theta out of range: correcting theta: " << refTrkPar[
Trk::theta];
117 }
119 {
120 if(
msgLvl(MSG::DEBUG))
msg(MSG::DEBUG)<<
" Theta out of range: correcting theta: " << refTrkPar[
Trk::theta];
123 }
124
126 {
136 }
137
138
139
140
141
142 const AmgSymMatrix(3)& vrt_cov = vtx.covariancePosition();
143 if (vrt_cov.determinant() == 0.0) {
144 ATH_MSG_WARNING(
"Matrix can not be inverted, new type of check as part of Eigen, please monitor."
145 <<
endmsg <<
"Matrix vrt_cov = " << vrt_cov);
146 return;
147 }
149 bool invertible;
151 vrt_weight = vtx.covariancePosition().inverse();
152 invertible = true;
153 } else
154 vtx.covariancePosition().computeInverseWithCheck(vrt_weight,invertible);
155
156 if(!invertible) {
157 ATH_MSG_VERBOSE (
"The vertex's cov is not invertible, quit updating the track.");
158 return;
159 }
160
161
162 AmgSymMatrix(3) posMomentumCovariance = -vrt_cov * A.transpose() * trkParametersWeight * B *Sm;
163
164
165
166
167 const IVertexUpdator::positionUpdateOutcome & reducedVrt =
m_Updator->positionUpdate( vtx, linTrack, trk.weight(), IVertexUpdator::removeTrack );
168
171 reduced_vrt_weight = reducedVrt.covariancePosition.inverse();
172 } else
173 reducedVrt.covariancePosition.computeInverseWithCheck(reduced_vrt_weight,invertible);
174 if(!invertible) {
175 ATH_MSG_VERBOSE (
"The vertex's cov is not invertible, quit updating the track.");
176 return;
177 }
178
179
180 Amg::Vector3D posDifference = vtx.position() - reducedVrt.position;
181
182
183 AmgVector(5) smRes = trkParameters - (theResidual + A*vtx.position() + B*newTrackMomentum);
184
185 double chi2 = posDifference.dot(reduced_vrt_weight*posDifference) + smRes.dot(trkParametersWeight*smRes);
186
187
188
189
190 AmgSymMatrix(3) momentumCovariance = Sm + posMomentumCovariance.transpose()*(vrt_weight*posMomentumCovariance);
191
192
194 fullTrkCov.setZero();
195
196 fullTrkCov.block<3,3>(0,0) = vrt_cov;
197 fullTrkCov.block<3,3>(0,3) = posMomentumCovariance;
198 fullTrkCov.block<3,3>(3,0) = posMomentumCovariance.transpose();
199 fullTrkCov.block<3,3>(3,3) = momentumCovariance;
200
201
203 trJac.setZero();
204 trJac(4,5) = 1.;
205 trJac(3,4) = 1.;
206 trJac(2,3) = 1.;
207
208
209 trJac(0,0) = - sin(refTrkPar[2]);
210 trJac(0,1) = cos(refTrkPar[2]);
211
212
213 trJac(1,0) = -cos(refTrkPar[2])/tan(refTrkPar[3]);
214 trJac(1,1) = -sin(refTrkPar[2])/tan(refTrkPar[3]);
215 trJac(1,2) = 1.;
216
218
219
220
221
222
224 refTrkPar[1],
225 refTrkPar[2],
226 refTrkPar[3],
227 refTrkPar[4],
228 fit_vrt_pos,
229 std::move(perFullTrkCov));
230
231 trk.setPerigeeAtVertex(refittedPerigee);
232 const FitQuality trkQuality(
chi2,2*trk.weight());
233
234
235 trk.setTrackQuality(trkQuality);
236
237 }
#define ATH_MSG_VERBOSE(x)
#define ATH_MSG_WARNING(x)
#define AmgSymMatrix(dim)
#define AmgMatrix(rows, cols)
bool msgLvl(const MSG::Level lvl) const
const Amg::Vector3D & position() const
Returns the 3-pos.
double chi2(TH1 *h0, TH1 *h1)
Eigen::Matrix< double, 3, 1 > Vector3D
ParametersT< TrackParametersDim, Charged, PerigeeSurface > Perigee