26 struct V0FitterTrack
final
28 V0FitterTrack() : originalPerigee(nullptr),
chi2(-1.) {}
29 ~V0FitterTrack() =
default;
39 TrkV0VertexFitter::TrkV0VertexFitter(
const std::string&
t,
const std::string&
n,
const IInterface*
p) : base_class(
t,
n,
p),
41 m_maxDchi2PerNdf(0.1),
46 m_extrapolator(
"Trk::Extrapolator/InDetExtrapolator", this)
50 declareProperty(
"MaxR",
m_maxR);
51 declareProperty(
"MaxZ",
m_maxZ);
53 declareProperty(
"Use_deltaR",
m_deltaR);
55 declareInterface<IVertexFitter>(
this);
64 return StatusCode::FAILURE;
72 return StatusCode::SUCCESS;
78 return StatusCode::SUCCESS;
86 std::vector<double>
masses;
87 double constraintMass = -9999.;
89 return fit(vectorTrk,
masses, constraintMass, pointingVertex, firstStartingPoint);
96 std::vector<double>
masses;
97 double constraintMass = -9999.;
100 return fit(vectorTrk,
masses, constraintMass, pointingVertex, startingPoint);
108 return fit(vectorTrk, tmpVtx);
113 const std::vector<double>&
masses,
114 const double& constraintMass,
118 const EventContext& ctx = Gaudi::Hive::currentContext();
119 std::vector<const Trk::TrackParameters*> measuredPerigees;
120 std::vector<const Trk::TrackParameters*> measuredPerigees_delete;
124 unsigned int indexFMP;
128 ATH_MSG_DEBUG(
"first measurement " <<
p->curvilinearParameters(indexFMP));
129 ATH_MSG_DEBUG(
"first measurement covariance " << *(
p->curvilinearParameters(indexFMP)).covariance());
132 CylTrf.setIdentity();
147 if (extrapolatedPerigee !=
nullptr) {
149 measuredPerigees.push_back (extrapolatedPerigee);
150 measuredPerigees_delete.push_back (extrapolatedPerigee);
153 extrapolatedPerigee =
162 if (extrapolatedPerigee !=
nullptr) {
163 ATH_MSG_DEBUG(
"extrapolated (direct) to first measurement");
164 measuredPerigees.push_back (extrapolatedPerigee);
165 measuredPerigees_delete.push_back (extrapolatedPerigee);
167 ATH_MSG_DEBUG(
"Failed to extrapolate to the first measurement on track, using Perigee parameters");
168 measuredPerigees.push_back (&
p->perigeeParameters());
173 measuredPerigees.push_back (&
p->perigeeParameters());
177 xAOD::Vertex * fittedVxCandidate =
fit(measuredPerigees,
masses, constraintMass, pointingVertex, firstStartingPoint);
180 if (fittedVxCandidate) {
189 for (
const auto *
ptr : measuredPerigees_delete){
delete ptr; }
191 return fittedVxCandidate;
200 std::vector<double>
masses;
201 double constraintMass = -9999.;
203 return fit(originalPerigees,
masses, constraintMass, pointingVertex, firstStartingPoint);
210 std::vector<double>
masses;
211 double constraintMass = -9999.;
214 return fit(originalPerigees,
masses, constraintMass, pointingVertex, startingPoint);
222 return fit(originalPerigees, tmpVtx);
227 const std::vector<double>&
masses,
228 const double& constraintMass,
232 const EventContext& ctx = Gaudi::Hive::currentContext();
233 if ( originalPerigees.empty() )
240 bool pointingConstraint =
false;
241 bool massConstraint =
false;
242 if(constraintMass > -100.) massConstraint =
true;
243 bool conversion =
false;
244 if(constraintMass == 0. && originalPerigees.size() == 2) conversion =
true;
245 double x_point=0., y_point=0., z_point=0.;
246 AmgSymMatrix(3) pointingVertexCov; pointingVertexCov.setIdentity();
247 if (pointingVertex !=
nullptr) {
248 if (pointingVertex->covariancePosition().trace() != 0.) {
249 pointingConstraint =
true;
254 pointingVertexCov = pointingVertex->covariancePosition().inverse();
259 msg(
MSG::DEBUG) <<
"massConstraint " << massConstraint <<
" pointingConstraint " << pointingConstraint <<
" conversion " << conversion <<
endmsg;
262 if (pointingConstraint)
msg(
MSG::DEBUG) <<
"pointing constraint, x = " << x_point <<
" y = " << y_point <<
" z = " << z_point <<
endmsg;
265 bool restartFit =
true;
266 double chi2 = 2000000000000.;
267 unsigned int nTrk = originalPerigees.size();
268 unsigned int nMeas = 5*nTrk;
269 unsigned int nVert = 1;
271 unsigned int nCnst = 2*nTrk;
272 unsigned int nPntC = 2;
273 unsigned int nMass = 1;
275 if (massConstraint) {
276 nCnst = nCnst + nMass;
278 if (pointingConstraint) {
279 nCnst = nCnst + nPntC;
284 unsigned int nPar = 5*nTrk + 3*nVert;
285 int ndf = nMeas - (nPar - nCnst);
288 unsigned int dim = nCnst;
289 unsigned int n_dim = nMeas;
293 std::vector<V0FitterTrack> v0FitterTracks;
299 Amg::MatrixX Wmeas_mat(n_dim,n_dim); Wmeas_mat.setZero();
300 Amg::MatrixX Wmeas0_mat(n_dim,n_dim); Wmeas0_mat.setZero();
324 const Amg::Vector3D * globalPosition = &(firstStartingPoint);
325 ATH_MSG_DEBUG(
"globalPosition of starting point: " << (*globalPosition)[0] <<
", " << (*globalPosition)[1] <<
", " << (*globalPosition)[2]);
327 if (globalPosition->perp() >
m_maxR && globalPosition->z() >
m_maxZ)
return nullptr;
330 if (!readHandle.isValid()) {
331 std::string
msg =
"Failed to retrieve magmnetic field conditions data ";
333 throw std::runtime_error(
msg);
338 fieldCondObj->getInitializedCache (fieldCache);
342 fieldCache.
getField(globalPosition->data(),BField);
343 double B_z = BField[2]*299.792;
344 if (B_z == 0. || std::isnan(B_z)) {
345 ATH_MSG_DEBUG(
"Could not find a magnetic field different from zero: very very strange");
348 ATH_MSG_VERBOSE(
"Magnetic field projection of z axis in the perigee position is: " << B_z <<
" GeV/mm ");
353 v0FitterTracks.clear();
358 if (chargeParameters !=
nullptr)
361 const Amg::Vector3D gMomentum = chargeParameters->momentum();
362 const Amg::Vector3D gDirection = chargeParameters->position() - *globalPosition;
363 const double extrapolationDirection = gMomentum.dot( gDirection );
366 std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(
nullptr);
368 std::unique_ptr<const Trk::TrackParameters>
tmp =
369 std::abs(chargeParameters->position().z()) >
m_maxZ ? nullptr :
380 extrapolatedPerigee.reset(
static_cast<const Trk::Perigee*
>(
tmp.release()));
383 if (extrapolatedPerigee ==
nullptr) {
384 ATH_MSG_DEBUG(
"Perigee was not extrapolated! Taking original one!");
386 if (tmpPerigee!=
nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
391 V0FitterTrack locV0FitterTrack;
392 locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[
Trk::d0];
393 locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[
Trk::z0];
394 locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[
Trk::phi];
395 locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[
Trk::theta];
396 locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[
Trk::qOverP];
397 locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
398 locV0FitterTrack.originalPerigee = chargeParameters;
399 v0FitterTracks.push_back(locV0FitterTrack);
401 ATH_MSG_DEBUG(
"Track parameters are not charged tracks ... fit aborted");
407 double chi2New=0.;
double chi2Old=
chi2;
409 bool onConstr =
false;
415 if (!restartFit) chi2Old = chi2New;
423 for (PTIter = v0FitterTracks.begin(); PTIter != v0FitterTracks.end() ; ++PTIter)
425 V0FitterTrack locP((*PTIter));
426 Wmeas0_mat.block(5*
i,5*
i,5,5) = locP.Wi_mat;
427 Wmeas_mat.block(5*
i,5*
i,5,5) = locP.Wi_mat;
428 for (
int j=0; j<5; ++j) {
429 Y0_vec(j+5*
i) = locP.TrkPar[j];
433 if(pointingConstraint) {
434 Y0_vec(5*nTrk + 0) = x_point;
435 Y0_vec(5*nTrk + 1) = y_point;
436 Y0_vec(5*nTrk + 2) = z_point;
437 Wmeas0_mat.block(5*nTrk,5*nTrk,3,3) = pointingVertexCov;
438 Wmeas_mat.block(5*nTrk,5*nTrk,3,3) = pointingVertexCov;
440 Wmeas_mat = Wmeas_mat.inverse();
443 Y_vec = Y0_vec + DeltaY_vec;
447 for (
unsigned int i=0;
i<nTrk; ++
i)
449 if ( fabs ( Y_vec(2+5*
i) ) > 100. || fabs ( Y_vec(3+5*
i) ) > 100. ) {
return nullptr; }
450 while ( fabs ( Y_vec(2+5*
i) ) >
M_PI ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) > 0 ) ? -2*
M_PI : 2*
M_PI;
451 while ( Y_vec(3+5*
i) > 2*
M_PI ) Y_vec(3+5*
i) -= 2*
M_PI;
452 while ( Y_vec(3+5*
i) < -
M_PI ) Y_vec(3+5*
i) +=
M_PI;
453 if ( Y_vec(3+5*
i) >
M_PI )
455 Y_vec(3+5*
i) = 2*
M_PI - Y_vec(3+5*
i);
456 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
458 if ( Y_vec(3+5*
i) < 0.0 )
460 Y_vec(3+5*
i) = - Y_vec(3+5*
i);
461 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
465 double SigE=0., SigPx=0., SigPy=0., SigPz=0., Px=0., Py=0., Pz=0.;
468 Amg::VectorX d0Cor(nTrk), d0Fac(nTrk), xcphiplusysphi(nTrk), xsphiminusycphi(nTrk);
469 d0Cor.setZero(); d0Fac.setZero(); xcphiplusysphi.setZero(); xsphiminusycphi.setZero();
471 conv_sign[0] = -1; conv_sign[1] = 1;
472 for (
unsigned int i=0;
i<nTrk; ++
i)
474 charge[
i] = (Y_vec(4+5*
i) < 0.) ? -1. : 1.;
475 rho[
i] =
sin(Y_vec(3+5*
i))/(B_z*Y_vec(4+5*
i));
476 xcphiplusysphi[
i] = A_vec(0)*
cos(Y_vec(2+5*
i))+A_vec(1)*
sin(Y_vec(2+5*
i));
477 xsphiminusycphi[
i] = A_vec(0)*
sin(Y_vec(2+5*
i))-A_vec(1)*
cos(Y_vec(2+5*
i));
478 if(fabs(-xcphiplusysphi[
i]/
rho[
i]) > 1.)
return nullptr;
479 d0Cor[
i] = 0.5*asin(-xcphiplusysphi[
i]/
rho[
i]);
480 double d0Facsq = 1. - xcphiplusysphi[
i]*xcphiplusysphi[
i]/(
rho[
i]*
rho[
i]);
481 d0Fac[
i] = (d0Facsq>0.) ? sqrt(d0Facsq) : 0;
482 Phi[
i] = Y_vec(2+5*
i) + 2.*d0Cor[
i];
495 double FMass=0., dFMassdxs=0., dFMassdys=0., dFMassdzs=0.;
496 double FPxy=0., dFPxydxs=0., dFPxydys=0., dFPxydzs=0., dFPxydxp=0., dFPxydyp=0., dFPxydzp=0.;
497 double FPxz=0., dFPxzdxs=0., dFPxzdys=0., dFPxzdzs=0., dFPxzdxp=0., dFPxzdyp=0., dFPxzdzp=0.;
499 Fxy.setZero(); Fxz.setZero(); dFMassdPhi.setZero();
500 Amg::VectorX drhodtheta(nTrk), drhodqOverP(nTrk), csplusbc(nTrk), ccminusbs(nTrk);
501 drhodtheta.setZero(); drhodqOverP.setZero(); csplusbc.setZero(); ccminusbs.setZero();
502 Amg::VectorX dFxydd0(nTrk), dFxydz0(nTrk), dFxydphi(nTrk), dFxydtheta(nTrk), dFxydqOverP(nTrk);
503 dFxydd0.setZero(); dFxydz0.setZero(); dFxydphi.setZero(); dFxydtheta.setZero(); dFxydqOverP.setZero();
504 Amg::VectorX dFxydxs(nTrk), dFxydys(nTrk), dFxydzs(nTrk);
505 dFxydxs.setZero(); dFxydys.setZero(); dFxydzs.setZero();
506 Amg::VectorX dFxzdd0(nTrk), dFxzdz0(nTrk), dFxzdphi(nTrk), dFxzdtheta(nTrk), dFxzdqOverP(nTrk);
507 dFxzdd0.setZero(); dFxzdz0.setZero(); dFxzdphi.setZero(); dFxzdtheta.setZero(); dFxzdqOverP.setZero();
508 Amg::VectorX dFxzdxs(nTrk), dFxzdys(nTrk), dFxzdzs(nTrk);
509 dFxzdxs.setZero(); dFxzdys.setZero(); dFxzdzs.setZero();
510 Amg::VectorX dFMassdd0(nTrk), dFMassdz0(nTrk), dFMassdphi(nTrk), dFMassdtheta(nTrk), dFMassdqOverP(nTrk);
511 dFMassdd0.setZero(); dFMassdz0.setZero(); dFMassdphi.setZero(); dFMassdtheta.setZero(); dFMassdqOverP.setZero();
512 Amg::VectorX dFPxydd0(nTrk), dFPxydz0(nTrk), dFPxydphi(nTrk), dFPxydtheta(nTrk), dFPxydqOverP(nTrk);
513 dFPxydd0.setZero(); dFPxydz0.setZero(); dFPxydphi.setZero(); dFPxydtheta.setZero(); dFPxydqOverP.setZero();
514 Amg::VectorX dFPxzdd0(nTrk), dFPxzdz0(nTrk), dFPxzdphi(nTrk), dFPxzdtheta(nTrk), dFPxzdqOverP(nTrk);
515 dFPxzdd0.setZero(); dFPxzdz0.setZero(); dFPxzdphi.setZero(); dFPxzdtheta.setZero(); dFPxzdqOverP.setZero();
516 Amg::VectorX dPhidd0(nTrk), dPhidz0(nTrk), dPhidphi0(nTrk), dPhidtheta(nTrk), dPhidqOverP(nTrk);
517 dPhidd0.setZero(); dPhidz0.setZero(); dPhidphi0.setZero(); dPhidtheta.setZero(); dPhidqOverP.setZero();
518 Amg::VectorX dPhidxs(nTrk), dPhidys(nTrk), dPhidzs(nTrk);
519 dPhidxs.setZero(); dPhidys.setZero(); dPhidzs.setZero();
528 FMass = constraintMass*constraintMass - SigE*SigE + SigPx*SigPx + SigPy*SigPy + SigPz*SigPz;
533 FPxy = Px*(frameOriginItr[1] - y_point) - Py*(frameOriginItr[0]- x_point);
537 FPxz = Px*(frameOriginItr[2] - z_point) - Pz*(frameOriginItr[0]- x_point);
539 for (
unsigned int i=0;
i<nTrk; ++
i)
544 Fxy[
i] = Y_vec(0+5*
i) + xsphiminusycphi[
i] - 2.*
rho[
i]*
sin(d0Cor[
i])*
sin(d0Cor[
i]);
548 Fxz[
i] = Y_vec(1+5*
i) - A_vec(2) -
rho[
i]*2.*d0Cor[
i]/
tan(Y_vec(3+5*
i));
552 drhodtheta[
i] =
cos(Y_vec(3+5*
i))/(B_z*Y_vec(4+5*
i));
553 drhodqOverP[
i] = -
sin(Y_vec(3+5*
i))/(B_z*Y_vec(4+5*
i)*Y_vec(4+5*
i));
556 dFxydphi[
i] = xcphiplusysphi[
i]*(1. + xsphiminusycphi[
i]/(d0Fac[
i]*
rho[
i]));
557 dFxydtheta[
i] = (xcphiplusysphi[
i]*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i])-2.*
sin(d0Cor[
i])*
sin(d0Cor[
i]))*drhodtheta[
i];
558 dFxydqOverP[
i] = (xcphiplusysphi[
i]*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i])-2.*
sin(d0Cor[
i])*
sin(d0Cor[
i]))*drhodqOverP[
i];
559 dFxydxs[
i] =
sin(Y_vec(2+5*
i)) -
cos(Y_vec(2+5*
i))*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]);
560 dFxydys[
i] = -
cos(Y_vec(2+5*
i)) -
sin(Y_vec(2+5*
i))*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]);
563 dFxzdphi[
i] = -xsphiminusycphi[
i]/(d0Fac[
i]*
tan(Y_vec(3+5*
i)));
564 dFxzdtheta[
i] = -((xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]) + 2.*d0Cor[
i])*
tan(Y_vec(3+5*
i))*drhodtheta[
i] -
566 dFxzdqOverP[
i] = -(xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]) + 2.*d0Cor[
i])*drhodqOverP[
i]/
tan(Y_vec(3+5*
i));
567 dFxzdxs[
i] =
cos(Y_vec(2+5*
i))/(d0Fac[
i]*
tan(Y_vec(3+5*
i)));
568 dFxzdys[
i] =
sin(Y_vec(2+5*
i))/(d0Fac[
i]*
tan(Y_vec(3+5*
i)));
571 dPhidphi0[
i] = 1. + xsphiminusycphi[
i]/(d0Fac[
i]*
rho[
i]);
572 dPhidtheta[
i] = xcphiplusysphi[
i]*drhodtheta[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i]);
573 dPhidqOverP[
i] = xcphiplusysphi[
i]*drhodqOverP[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i]);
574 dPhidxs[
i] = -
cos(Y_vec(2+5*
i))/(d0Fac[
i]*
rho[
i]);
575 dPhidys[
i] = -
sin(Y_vec(2+5*
i))/(d0Fac[
i]*
rho[
i]);
579 dFMassdphi[
i] = conv_sign[
i]*dPhidphi0[
i];
580 dFMassdtheta[
i] = conv_sign[
i]*dPhidtheta[
i];
581 dFMassdqOverP[
i] = conv_sign[
i]*dPhidqOverP[
i];
582 dFMassdxs += conv_sign[
i]*dPhidxs[
i];
583 dFMassdys += conv_sign[
i]*dPhidys[
i];
585 csplusbc[
i] = SigPy*
sin(Y_vec(2+5*
i))+SigPx*
cos(Y_vec(2+5*
i));
586 ccminusbs[
i] = SigPy*
cos(Y_vec(2+5*
i))-SigPx*
sin(Y_vec(2+5*
i));
587 dFMassdphi[
i] = 2.*
sin(Y_vec(3+5*
i))*ccminusbs[
i]*
charge[
i]/Y_vec(4+5*
i);
588 dFMassdtheta[
i] = 2.*(
cos(Y_vec(3+5*
i))*csplusbc[
i] -
sin(Y_vec(3+5*
i))*SigPz)*
charge[
i]/Y_vec(4+5*
i);
589 dFMassdqOverP[
i] = 2.*SigE/(sqrt(1./(Y_vec(4+5*
i)*Y_vec(4+5*
i)) +
masses[
i]*
masses[
i])*Y_vec(4+5*
i)*Y_vec(4+5*
i)*Y_vec(4+5*
i)) -
590 2.*
charge[
i]*(
sin(Y_vec(3+5*
i))*csplusbc[
i] +
cos(Y_vec(3+5*
i))*SigPz)/(Y_vec(4+5*
i)*Y_vec(4+5*
i));
594 if (pointingConstraint){
595 dFPxydphi[
i] = -
sin(Y_vec(3+5*
i))*(
sin(Y_vec(2+5*
i))*(frameOriginItr[1]-y_point)+
cos(Y_vec(2+5*
i))*(frameOriginItr[0]-x_point))*
charge[
i]/Y_vec(4+5*
i);
596 dFPxydtheta[
i] =
cos(Y_vec(3+5*
i))*(
cos(Y_vec(2+5*
i))*(frameOriginItr[1]-y_point)-
sin(Y_vec(2+5*
i))*(frameOriginItr[0]-x_point))*
charge[
i]/Y_vec(4+5*
i);
597 dFPxydqOverP[
i] = -
sin(Y_vec(3+5*
i))*(
cos(Y_vec(2+5*
i))*(frameOriginItr[1]-y_point)-
sin(Y_vec(2+5*
i))*(frameOriginItr[0]-x_point))*
charge[
i]/(Y_vec(4+5*
i)*Y_vec(4+5*
i));
603 dFPxzdphi[
i] = -
sin(Y_vec(3+5*
i))*
sin(Y_vec(2+5*
i))*(frameOriginItr[2]-z_point)*
charge[
i]/Y_vec(4+5*
i);
604 dFPxzdtheta[
i] =
cos(Y_vec(3+5*
i))*
cos(Y_vec(2+5*
i))*(frameOriginItr[2]-z_point)*
charge[
i]/Y_vec(4+5*
i)
605 +
sin(Y_vec(3+5*
i))*(frameOriginItr[0]-x_point)*
charge[
i]/Y_vec(4+5*
i);
606 dFPxzdqOverP[
i] = -
sin(Y_vec(3+5*
i))*
cos(Y_vec(2+5*
i))*(frameOriginItr[2]-z_point)*
charge[
i]/(Y_vec(4+5*
i)*Y_vec(4+5*
i))
607 +
cos(Y_vec(3+5*
i))*(frameOriginItr[0]-x_point)*
charge[
i]/(Y_vec(4+5*
i)*Y_vec(4+5*
i));
616 F_vec[
i+nTrk] = -Fxz[
i];
618 F_fac_vec[
i+nTrk] = 1.;
620 if(massConstraint) F_vec(2*nTrk+0) = -FMass;
622 if(massConstraint) F_fac_vec(2*nTrk+0) = 0.000001;
623 if(pointingConstraint) {
625 F_vec(2*nTrk+1) = -FPxy;
626 F_vec(2*nTrk+2) = -FPxz;
627 F_fac_vec(2*nTrk+1) = 0.000001;
628 F_fac_vec(2*nTrk+2) = 0.000001;
630 F_vec(2*nTrk+0) = -FPxy;
631 F_vec(2*nTrk+1) = -FPxz;
632 F_fac_vec(2*nTrk+0) = 0.000001;
633 F_fac_vec(2*nTrk+1) = 0.000001;
638 for (
unsigned int i=0;
i<
dim; ++
i)
640 sumConstr += F_fac_vec[
i]*fabs(F_vec[
i]);
642 if ( std::isnan(sumConstr) ) {
return nullptr; }
643 if (sumConstr < 0.001) { onConstr =
true; }
646 for (
unsigned int i=0;
i<nTrk; ++
i)
648 Bjac_mat(
i,0+5*
i) = dFxydd0(
i);
649 Bjac_mat(
i,1+5*
i) = dFxydz0(
i);
650 Bjac_mat(
i,2+5*
i) = dFxydphi(
i);
651 Bjac_mat(
i,3+5*
i) = dFxydtheta(
i);
652 Bjac_mat(
i,4+5*
i) = dFxydqOverP(
i);
653 Bjac_mat(
i+nTrk,0+5*
i) = dFxzdd0(
i);
654 Bjac_mat(
i+nTrk,1+5*
i) = dFxzdz0(
i);
655 Bjac_mat(
i+nTrk,2+5*
i) = dFxzdphi(
i);
656 Bjac_mat(
i+nTrk,3+5*
i) = dFxzdtheta(
i);
657 Bjac_mat(
i+nTrk,4+5*
i) = dFxzdqOverP(
i);
659 Bjac_mat(2*nTrk,0+5*
i) = dFMassdd0(
i);
660 Bjac_mat(2*nTrk,1+5*
i) = dFMassdz0(
i);
661 Bjac_mat(2*nTrk,2+5*
i) = dFMassdphi(
i);
662 Bjac_mat(2*nTrk,3+5*
i) = dFMassdtheta(
i);
663 Bjac_mat(2*nTrk,4+5*
i) = dFMassdqOverP(
i);
665 if(pointingConstraint) {
667 Bjac_mat(2*nTrk+1,0+5*
i) = dFPxydd0(
i);
668 Bjac_mat(2*nTrk+1,1+5*
i) = dFPxydz0(
i);
669 Bjac_mat(2*nTrk+1,2+5*
i) = dFPxydphi(
i);
670 Bjac_mat(2*nTrk+1,3+5*
i) = dFPxydtheta(
i);
671 Bjac_mat(2*nTrk+1,4+5*
i) = dFPxydqOverP(
i);
672 Bjac_mat(2*nTrk+1,5*nTrk) = dFPxydxp;
673 Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxydyp;
674 Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxydzp;
675 Bjac_mat(2*nTrk+2,0+5*
i) = dFPxzdd0(
i);
676 Bjac_mat(2*nTrk+2,1+5*
i) = dFPxzdz0(
i);
677 Bjac_mat(2*nTrk+2,2+5*
i) = dFPxzdphi(
i);
678 Bjac_mat(2*nTrk+2,3+5*
i) = dFPxzdtheta(
i);
679 Bjac_mat(2*nTrk+2,4+5*
i) = dFPxzdqOverP(
i);
680 Bjac_mat(2*nTrk+2,5*nTrk) = dFPxzdxp;
681 Bjac_mat(2*nTrk+2,5*nTrk+1) = dFPxzdyp;
682 Bjac_mat(2*nTrk+2,5*nTrk+2) = dFPxzdzp;
684 Bjac_mat(2*nTrk+0,0+5*
i) = dFPxydd0(
i);
685 Bjac_mat(2*nTrk+0,1+5*
i) = dFPxydz0(
i);
686 Bjac_mat(2*nTrk+0,2+5*
i) = dFPxydphi(
i);
687 Bjac_mat(2*nTrk+0,3+5*
i) = dFPxydtheta(
i);
688 Bjac_mat(2*nTrk+0,4+5*
i) = dFPxydqOverP(
i);
689 Bjac_mat(2*nTrk+0,5*nTrk) = dFPxydxp;
690 Bjac_mat(2*nTrk+0,5*nTrk+1) = dFPxydyp;
691 Bjac_mat(2*nTrk+0,5*nTrk+2) = dFPxydzp;
692 Bjac_mat(2*nTrk+1,0+5*
i) = dFPxzdd0(
i);
693 Bjac_mat(2*nTrk+1,1+5*
i) = dFPxzdz0(
i);
694 Bjac_mat(2*nTrk+1,2+5*
i) = dFPxzdphi(
i);
695 Bjac_mat(2*nTrk+1,3+5*
i) = dFPxzdtheta(
i);
696 Bjac_mat(2*nTrk+1,4+5*
i) = dFPxzdqOverP(
i);
697 Bjac_mat(2*nTrk+1,5*nTrk) = dFPxzdxp;
698 Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxzdyp;
699 Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxzdzp;
703 Ajac_mat(
i,0) = dFxydxs(
i);
704 Ajac_mat(
i,1) = dFxydys(
i);
705 Ajac_mat(
i,2) = dFxydzs(
i);
706 Ajac_mat(
i+nTrk,0) = dFxzdxs(
i);
707 Ajac_mat(
i+nTrk,1) = dFxzdys(
i);
708 Ajac_mat(
i+nTrk,2) = dFxzdzs(
i);
710 Ajac_mat(2*nTrk,0) = dFMassdxs;
711 Ajac_mat(2*nTrk,1) = dFMassdys;
712 Ajac_mat(2*nTrk,2) = dFMassdzs;
714 if(pointingConstraint) {
716 Ajac_mat(2*nTrk+1,0) = dFPxydxs;
717 Ajac_mat(2*nTrk+1,1) = dFPxydys;
718 Ajac_mat(2*nTrk+1,2) = dFPxydzs;
719 Ajac_mat(2*nTrk+2,0) = dFPxzdxs;
720 Ajac_mat(2*nTrk+2,1) = dFPxzdys;
721 Ajac_mat(2*nTrk+2,2) = dFPxzdzs;
723 Ajac_mat(2*nTrk+0,0) = dFPxydxs;
724 Ajac_mat(2*nTrk+0,1) = dFPxydys;
725 Ajac_mat(2*nTrk+0,2) = dFPxydzs;
726 Ajac_mat(2*nTrk+1,0) = dFPxzdxs;
727 Ajac_mat(2*nTrk+1,1) = dFPxzdys;
728 Ajac_mat(2*nTrk+1,2) = dFPxzdzs;
733 Wb_mat = Wmeas_mat.similarity(Bjac_mat) ;
734 Wb_mat = Wb_mat.inverse();
736 C22_mat = Wb_mat.similarity(Ajac_mat.transpose());
737 C22_mat = C22_mat.inverse();
739 Btemp_mat = Wb_mat * Bjac_mat * Wmeas_mat;
740 Atemp_mat = Wb_mat * Ajac_mat;
742 C21_mat = - C22_mat * Ajac_mat.transpose() * Btemp_mat;
743 C32_mat = Atemp_mat * C22_mat;
744 C31_mat = Btemp_mat + Atemp_mat * C21_mat;
745 Amg::MatrixX mat_prod_1 = Wmeas_mat * Bjac_mat.transpose();
746 Amg::MatrixX mat_prod_2 = Wmeas_mat * Bjac_mat.transpose() * Wb_mat * Ajac_mat;
747 C11_mat = Wmeas_mat - Wb_mat.similarity( mat_prod_1 ) + C22_mat.similarity( mat_prod_2 );
749 C_cor_vec = Ajac_mat*DeltaA_vec + Bjac_mat*DeltaY_vec;
750 C_vec = C_cor_vec + F_vec;
752 DeltaY_vec = C31_mat.transpose()*C_vec;
753 DeltaA_vec = C32_mat.transpose()*C_vec;
755 for (
unsigned int i=0;
i<n_dim; ++
i)
757 ChiItr_vec(0,
i) = DeltaY_vec(
i);
759 ChiItr_mat = Wmeas0_mat.similarity( ChiItr_vec );
760 chi2New = ChiItr_mat(0,0);
763 frameOriginItr[0] += DeltaA_vec(0);
764 frameOriginItr[1] += DeltaA_vec(1);
765 frameOriginItr[2] += DeltaA_vec(2);
767 msg(
MSG::DEBUG) <<
"New vertex, global coordinates: " << frameOriginItr.transpose() <<
endmsg;
768 msg(
MSG::DEBUG) <<
"chi2Old: " << chi2Old <<
" chi2New: " << chi2New <<
" fabs(chi2Old-chi2New): " << fabs(chi2Old-chi2New) <<
endmsg;
772 if (globalPositionItr->perp() >
m_maxR && globalPositionItr->z() >
m_maxZ)
return nullptr;
774 if (onConstr && fabs(chi2Old-chi2New) < 0.1) {
break; }
777 fieldCache.
getField(globalPositionItr->data(),BFieldItr);
778 double B_z_new = BFieldItr[2]*299.792;
779 if (B_z_new == 0. || std::isnan(B_z)) {
785 double deltaR = sqrt(DeltaA_vec(0)*DeltaA_vec(0)+DeltaA_vec(1)*DeltaA_vec(1)+DeltaA_vec(2)*DeltaA_vec(2));
786 double deltaB_z = fabs(B_z-B_z_new)/B_z;
787 bool changeBz =
false;
798 v0FitterTracks.clear();
803 if (chargeParameters !=
nullptr)
806 const Amg::Vector3D gMomentum = chargeParameters->momentum();
807 const Amg::Vector3D gDirection = chargeParameters->position() - *globalPositionItr;
808 const double extrapolationDirection = gMomentum .dot( gDirection );
811 std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(
nullptr);
813 std::unique_ptr<const Trk::TrackParameters>
tmp =
814 std::abs(chargeParameters->position().z()) >
m_maxZ ? nullptr :
825 extrapolatedPerigee.reset(
829 if (extrapolatedPerigee ==
nullptr) {
830 ATH_MSG_DEBUG(
"Perigee was not extrapolated! Taking original one!");
832 if (tmpPerigee!=
nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
837 V0FitterTrack locV0FitterTrack;
838 locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[
Trk::d0];
839 locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[
Trk::z0];
840 locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[
Trk::phi];
841 locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[
Trk::theta];
842 locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[
Trk::qOverP];
843 locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
844 locV0FitterTrack.originalPerigee = chargeParameters;
845 v0FitterTracks.push_back(locV0FitterTrack);
847 ATH_MSG_DEBUG(
"Track parameters are not charged tracks ... fit aborted");
851 frameOrigin = frameOriginItr;
857 chi2Old = 2000000000000.;
868 frameOrigin[0] += DeltaA_vec(0);
869 frameOrigin[1] += DeltaA_vec(1);
870 frameOrigin[2] += DeltaA_vec(2);
871 if ( std::isnan(frameOrigin[0]) || std::isnan(frameOrigin[1]) || std::isnan(frameOrigin[2]) )
return nullptr;
873 Y_vec = Y0_vec + DeltaY_vec;
876 for (
unsigned int i=0;
i<nTrk; ++
i)
878 if ( fabs ( Y_vec(2+5*
i) ) > 100. || fabs ( Y_vec(3+5*
i) ) > 100. ) {
return nullptr; }
879 while ( fabs ( Y_vec(2+5*
i) ) >
M_PI ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) > 0 ) ? -2*
M_PI : 2*
M_PI;
880 while ( Y_vec(3+5*
i) > 2*
M_PI ) Y_vec(3+5*
i) -= 2*
M_PI;
881 while ( Y_vec(3+5*
i) < -
M_PI ) Y_vec(3+5*
i) +=
M_PI;
882 if ( Y_vec(3+5*
i) >
M_PI )
884 Y_vec(3+5*
i) = 2*
M_PI - Y_vec(3+5*
i);
885 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
887 if ( Y_vec(3+5*
i) < 0.0 )
889 Y_vec(3+5*
i) = - Y_vec(3+5*
i);
890 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
894 for (
unsigned int i=0;
i<n_dim; ++
i)
896 Chi_vec(0,
i) = DeltaY_vec(
i);
898 Chi_mat = Wmeas0_mat.similarity( Chi_vec );
902 V_mat.block(0,0,n_dim,n_dim) = C11_mat;
903 V_mat.block(n_dim,n_dim,3,3) = C22_mat;
904 V_mat.block(n_dim,0,3,n_dim) = C21_mat;
905 V_mat.block(0,n_dim,n_dim,3) = C21_mat.transpose();
910 for (BTIter = v0FitterTracks.begin(); BTIter != v0FitterTracks.end() ; ++BTIter)
914 covTrk = Wmeas0_mat.block(5*iRP,5*iRP,4+5*iRP,4+5*iRP);
916 for (
unsigned int i=0;
i<5; ++
i) chi_vec(
i) = DeltaY_vec(
i+5*iRP);
917 double chi2Trk = chi_vec.dot(covTrk*chi_vec);
918 (*BTIter).chi2=chi2Trk;
931 std::vector<VxTrackAtVertex> & tracksAtVertex = vx->
vxTrackAtVertex(); tracksAtVertex.clear();
935 unsigned int iterf=0;
937 for (BTIterf = v0FitterTracks.begin(); BTIterf != v0FitterTracks.end() ; ++BTIterf)
940 CovMtxP.setIdentity();
941 for (
unsigned int i=0;
i<5; ++
i) {
942 for (
unsigned int j=0; j<
i+1; ++j) {
943 double val = V_mat(5*iterf+
i,5*iterf+j);
944 CovMtxP.fillSymmetric(
i,j,
val);
947 refittedPerigee =
new Trk::Perigee (Y_vec(0+5*iterf),Y_vec(1+5*iterf),Y_vec(2+5*iterf),Y_vec(3+5*iterf),Y_vec(4+5*iterf),
949 tracksAtVertex.emplace_back((*BTIterf).chi2, refittedPerigee, (*BTIterf).originalPerigee);
954 unsigned int sfcmv = nPar*(nPar+1)/2;
955 std::vector<float> floatErrMtx(sfcmv,0.);
956 unsigned int ipnt = 0;
957 for (
unsigned int i=0;
i<nPar; ++
i) {
958 for (
unsigned int j=0; j<
i+1; ++j) {
959 floatErrMtx[ipnt++]=V_mat(
i,j);