22 int numRow(
int numVertex) {
37 return StatusCode::FAILURE;
39 m_initial_cov_momentum.setZero();
44 return StatusCode::SUCCESS;
50 m_Updator(
"Trk::KalmanVertexOnJetAxisUpdator", this),
52 m_initialMomentumError(1000.)
57 declareInterface<KalmanVertexOnJetAxisSmoother>(
this);
66 bool updateTrack)
const {
71 update(vertexToSmooth,
isPrimary,myFinalPosition,linearizationPositions,updateTrack,
false);
77 bool updateTrack)
const {
81 update(vertexToSmooth,
isPrimary,myFinalPosition,linearizationPositions,updateTrack,
true);
87 bool updateTrack,
bool doFastUpdate)
const {
92 if (vertexToSmooth==
nullptr) {
93 ATH_MSG_WARNING(
" Empty pointers then calling fit method update. No fit will be done..." );
98 ATH_MSG_DEBUG(
"It's " << (updateTrack?
"with":
"without") <<
" updating the track.");
99 ATH_MSG_DEBUG(
"It's the "<<(doFastUpdate?
"fast":
"normal (weight-formalism)")<<
" smoother");
101 const std::vector<VxTrackAtVertex*> & allTracksToSmooth(vertexToSmooth->
getTracksAtVertex());
106 if (allTracksToSmooth.empty()) {
109 ATH_MSG_WARNING (
"Nothing to smooth and it's not a primary vertex: BUG... ");
119 std::pair<Amg::Vector3D,Eigen::Matrix3Xd> PosOnJetAxisAndTransformMatrix =
122 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
123 std::cout <<
" smoother: " << PosOnJetAxisAndTransformMatrix.first <<
" and transform matrix: " << PosOnJetAxisAndTransformMatrix.second <<
141 Eigen::FullPivLU<Amg::MatrixX> lu_decomp(final_vrt_covariance);
142 if (!lu_decomp.isInvertible()) {
143 ATH_MSG_DEBUG(
"Jet vertex positions matrix not invertible right at start of smoother!" <<
144 " -> stop smoothing.");
147 final_vrt_weightmatrix = final_vrt_covariance.inverse();
148 myPositionInWeightFormalism.
setPosition(final_vrt_weightmatrix*
149 myPositionInWeightFormalism.
position());
154 double track_compatibility_chi2(0.);
155 double track_ndf(0.);
159 const std::vector<VxTrackAtVertex*>::const_iterator TracksBegin=allTracksToSmooth.begin();
160 const std::vector<VxTrackAtVertex*>::const_iterator TracksEnd=allTracksToSmooth.end();
162 for (std::vector<VxTrackAtVertex*>::const_iterator TracksIter=TracksBegin;TracksIter!=TracksEnd;++TracksIter) {
168 ATH_MSG_WARNING (
" Empty pointers then calling smoothing method update. No smoothing will be performed...");
172 double trackWeight = (*TracksIter)->weight();
175 const AmgMatrix(5,3)& oldA = trk->positionJacobian();
177 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
178 std::cout <<
"the old jacobian xyz d0z0phithetaqoverp vs xyz " << oldA << std::endl;
182 Eigen::Matrix<double,5,Eigen::Dynamic>
A = oldA*PosOnJetAxisAndTransformMatrix.second;
184 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
185 std::cout <<
"the new jacobian " <<
A << std::endl;
188 const AmgMatrix(5,3) & B = trk->momentumJacobian();
189 const AmgVector(5)& trackParameters = trk->expectedParametersAtPCA();
191 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
192 std::cout <<
"expected perigee at pca " << trackParameters << std::endl;
194 std::cout <<
" constant term " << trk->constantTerm() <<
195 " old A " << oldA <<
" * PosOnJetAxis " << PosOnJetAxisAndTransformMatrix.first <<
196 " A " <<
A <<
" * initialjetdir " << initialjetdir << std::endl;
199 AmgVector(5) constantTerm=trk->constantTerm() + oldA*PosOnJetAxisAndTransformMatrix.first
201 const AmgSymMatrix(5)& trackParametersWeight = trk->expectedWeightAtPCA();
203 if(
S.determinant()==0.0) {
204 ATH_MSG_WARNING (
"The S matrix inversion failed during smoothing iteration cycle");
207 S=
S.inverse().eval();
213 AmgSymMatrix(5) old_residual_cov = -trk->expectedCovarianceAtPCA() +
214 A*old_vrt_cov*
A.transpose() + B*m_initial_cov_momentum*B.transpose();
215 if (old_residual_cov.determinant() == 0.0 ) {
216 ATH_MSG_ERROR (
"The old_residual matrix can not be inverted during smoothing interation cycle");
219 AmgSymMatrix(5) old_residual_cov_inv = old_residual_cov.inverse().eval();
220 Eigen::Matrix<double,Eigen::Dynamic,5> Kk1=old_vrt_cov*
A.transpose()*old_residual_cov_inv;
221 AmgVector(5) residual_vector=trackParameters-constantTerm-
A*old_vrt_pos;
222 Amg::VectorX new_vrt_pos = old_vrt_pos+Kk1*residual_vector;
223 Amg::MatrixX new_vrt_cov = old_vrt_cov+Kk1*old_residual_cov*Kk1.transpose()
224 - 2.*Kk1*
A*old_vrt_cov;
231 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
232 std::cout <<
" Old weight Matrix is " << myPositionInWeightFormalism.
covariancePosition() << std::endl;
240 AmgSymMatrix(5) gB = trackParametersWeight - trackParametersWeight*(B*(
S*B.transpose()))*trackParametersWeight.transpose();
243 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
244 std::cout<<
"Gain factor obtained: "<<trackParametersWeight*(B*(
S*B.transpose()))*trackParametersWeight.transpose()<<std::endl;
245 std::cout<<
"Resulting Gain Matrix: "<<gB<<std::endl;
249 Amg::MatrixX new_vrt_weight = old_vrt_weight - trackWeight *
A.transpose()*gB*
A;
251 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
252 std::cout<<
"New vertex weight obtained: "<<new_vrt_weight<<std::endl;
256 Amg::VectorX new_weight_matrix_times_vrt_position =old_vrt_weight_times_vrt_pos - trackWeight *
A.transpose() * gB *(trackParameters - constantTerm);
260 myPositionInWeightFormalism =
265 Amg::Vector3D newTrackMomentum =
S*B.transpose()*trackParametersWeight*(trackParameters - constantTerm -
A*final_vrt_pos);
267 AmgVector(5) refTrackParameters = constantTerm +
A * final_vrt_pos + B * newTrackMomentum;
272 (PosOnJetAxisAndTransformMatrix.second)*final_vrt_covariance*(PosOnJetAxisAndTransformMatrix.second).transpose();
274 AmgVector(5) refTrackParametersToStore(refTrackParameters);
299 else if (refTrackParametersToStore[
Trk::theta]<0) {
305 if (refTrackParametersToStore[
Trk::theta] < 0) {
323 AmgMatrix(
Eigen::Dynamic,3) posMomentumCovariance_xyz = PosOnJetAxisAndTransformMatrix.second * posMomentumCovariance;
327 S + (partialCovariance.transpose()*final_vrt_covariance*partialCovariance) :
328 S + (posMomentumCovariance.transpose()*final_vrt_weightmatrix*posMomentumCovariance);
333 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
334 std::cout <<
" posMomentumCovariance " << posMomentumCovariance <<
335 " final_vrt_covariance " << final_vrt_covariance <<
" momentumCovariance " <<
336 momentumCovariance << std::endl;
339 fullTrkCov.block<3,3>(0,3) = posMomentumCovariance_xyz.transpose();
340 fullTrkCov.block<3,3>(3,0) = posMomentumCovariance_xyz;
341 fullTrkCov.block<3,3>(0,0) = final_vrt_covariance_xyz;
342 fullTrkCov.block<3,3>(3,3) = momentumCovariance;
350 trJac(0,0) = -
sin(refTrackParametersToStore[
Trk::phi0]);
361 refTrackParametersToStore[1],
362 refTrackParametersToStore[2],
363 refTrackParametersToStore[3],
364 refTrackParametersToStore[4],
366 std::move(perFullTrkCov));
369 (*TracksIter)->setPerigeeAtVertex(refittedPerigee);
374 AmgVector(5) paramDifference = trackParameters - refTrackParameters;
376 track_compatibility_chi2 +=
377 (paramDifference.transpose()*trackParametersWeight*paramDifference)(0,0)*trackWeight;
379 ATH_MSG_DEBUG (
"new chi2 with compatibility of the tracks to the vertices " <<
380 track_compatibility_chi2 <<
" new ndf: " << track_ndf);
388 const Amg::VectorX & vrt_removed_tracks_weight_times_vrt_pos =
389 myPositionInWeightFormalism.
position();
393 Amg::MatrixX vrt_removed_tracks_covariance = vrt_removed_tracks_weight;
397 vrt_removed_tracks_covariance=(vrt_removed_tracks_covariance.transpose().eval()+vrt_removed_tracks_covariance)/2.0;
401 m_Updator->smartInvert(vrt_removed_tracks_covariance);
403 catch (std::string
a)
406 vrt_removed_tracks_covariance=vrt_removed_tracks_weight.inverse().eval();
409 const Amg::VectorX vrt_removed_tracks_pos=vrt_removed_tracks_covariance*vrt_removed_tracks_weight_times_vrt_pos;
411 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
412 std::cout <<
" final_vrt_pos " << final_vrt_pos <<
" vrt_removed_tracks_pos " << vrt_removed_tracks_pos << std::endl;
413 std::cout <<
" weight removed tracks " << vrt_removed_tracks_weight << std::endl;
416 track_compatibility_chi2 +=
417 ((final_vrt_pos-vrt_removed_tracks_pos).transpose()*vrt_removed_tracks_weight*(final_vrt_pos-vrt_removed_tracks_pos))(0,0);
420 ATH_MSG_DEBUG (
" new chi2 with the vertex residual" << track_compatibility_chi2 <<
421 " new ndf: " << track_ndf);
424 #ifdef KalmanVertexOnJetAxisSmoother_DEBUG
425 std::cout <<
" vertexToSmooth pointer is " << vertexToSmooth << std::endl;