226 const EventContext& ctx = Gaudi::Hive::currentContext();
227 if ( originalPerigees.empty() )
234 bool pointingConstraint =
false;
235 bool massConstraint =
false;
236 if(constraintMass > -100.) massConstraint =
true;
237 bool conversion =
false;
238 if(constraintMass == 0. && originalPerigees.size() == 2) conversion =
true;
239 double x_point=0., y_point=0., z_point=0.;
240 AmgSymMatrix(3) pointingVertexCov; pointingVertexCov.setIdentity();
241 if (pointingVertex !=
nullptr) {
242 if (pointingVertex->covariancePosition().trace() != 0.) {
243 pointingConstraint =
true;
248 pointingVertexCov = pointingVertex->covariancePosition().inverse();
253 msg(
MSG::DEBUG) <<
"massConstraint " << massConstraint <<
" pointingConstraint " << pointingConstraint <<
" conversion " << conversion <<
endmsg;
256 if (pointingConstraint)
msg(
MSG::DEBUG) <<
"pointing constraint, x = " << x_point <<
" y = " << y_point <<
" z = " << z_point <<
endmsg;
259 bool restartFit =
true;
260 double chi2 = 2000000000000.;
261 unsigned int nTrk = originalPerigees.size();
262 unsigned int nMeas = 5*nTrk;
263 unsigned int nVert = 1;
265 unsigned int nCnst = 2*nTrk;
266 unsigned int nPntC = 2;
267 unsigned int nMass = 1;
269 if (massConstraint) {
270 nCnst = nCnst + nMass;
272 if (pointingConstraint) {
273 nCnst = nCnst + nPntC;
278 unsigned int nPar = 5*nTrk + 3*nVert;
279 int ndf = nMeas - (nPar - nCnst);
282 unsigned int dim = nCnst;
283 unsigned int n_dim = nMeas;
287 std::vector<V0FitterTrack> v0FitterTracks;
293 Amg::MatrixX Wmeas_mat(n_dim,n_dim); Wmeas_mat.setZero();
294 Amg::MatrixX Wmeas0_mat(n_dim,n_dim); Wmeas0_mat.setZero();
314 Amg::
MatrixX ChiItr_vec(1,n_dim); ChiItr_vec.setZero();
319 ATH_MSG_DEBUG("globalPosition
of starting point: " << (*globalPosition)[0] << ", " << (*globalPosition)[1] << ", " << (*globalPosition)[2]);
323 if (!readHandle.isValid()) {
324 std::string
msg =
"Failed to retrieve magmnetic field conditions data ";
326 throw std::runtime_error(
msg);
331 fieldCondObj->getInitializedCache (fieldCache);
335 fieldCache.
getField(globalPosition->data(),BField);
336 double B_z = BField[2]*299.792;
337 if (B_z == 0. || std::isnan(B_z)) {
338 ATH_MSG_DEBUG(
"Could not find a magnetic field different from zero: very very strange");
341 ATH_MSG_VERBOSE(
"Magnetic field projection of z axis in the perigee position is: " << B_z <<
" GeV/mm ");
346 v0FitterTracks.clear();
351 if (chargeParameters !=
nullptr)
354 const Amg::Vector3D gMomentum = chargeParameters->momentum();
355 const Amg::Vector3D gDirection = chargeParameters->position() - *globalPosition;
356 const double extrapolationDirection = gMomentum.dot( gDirection );
359 std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(
nullptr);
360 std::unique_ptr<const Trk::TrackParameters>
tmp =
370 extrapolatedPerigee.reset(
static_cast<const Trk::Perigee*
>(
tmp.release()));
373 if (extrapolatedPerigee ==
nullptr) {
374 ATH_MSG_DEBUG(
"Perigee was not extrapolated! Taking original one!");
376 if (tmpPerigee!=
nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
381 V0FitterTrack locV0FitterTrack;
382 locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[
Trk::d0];
383 locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[
Trk::z0];
384 locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[
Trk::phi];
385 locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[
Trk::theta];
386 locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[
Trk::qOverP];
387 locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
388 locV0FitterTrack.originalPerigee = chargeParameters;
389 v0FitterTracks.push_back(locV0FitterTrack);
391 ATH_MSG_DEBUG(
"Track parameters are not charged tracks ... fit aborted");
397 double chi2New=0.;
double chi2Old=
chi2;
399 bool onConstr =
false;
405 if (!restartFit) chi2Old = chi2New;
413 for (PTIter = v0FitterTracks.begin(); PTIter != v0FitterTracks.end() ; ++PTIter)
415 V0FitterTrack locP((*PTIter));
416 Wmeas0_mat.block(5*
i,5*
i,5,5) = locP.Wi_mat;
417 Wmeas_mat.block(5*
i,5*
i,5,5) = locP.Wi_mat;
418 for (
int j=0; j<5; ++j) {
419 Y0_vec(j+5*
i) = locP.TrkPar[j];
423 if(pointingConstraint) {
424 Y0_vec(5*nTrk + 0) = x_point;
425 Y0_vec(5*nTrk + 1) = y_point;
426 Y0_vec(5*nTrk + 2) = z_point;
427 Wmeas0_mat.block(5*nTrk,5*nTrk,3,3) = pointingVertexCov;
428 Wmeas_mat.block(5*nTrk,5*nTrk,3,3) = pointingVertexCov;
430 Wmeas_mat = Wmeas_mat.inverse();
433 Y_vec = Y0_vec + DeltaY_vec;
437 for (
unsigned int i=0;
i<nTrk; ++
i)
439 if ( fabs ( Y_vec(2+5*
i) ) > 100. || fabs ( Y_vec(3+5*
i) ) > 100. ) {
return nullptr; }
440 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;
441 while ( Y_vec(3+5*
i) > 2*
M_PI ) Y_vec(3+5*
i) -= 2*
M_PI;
442 while ( Y_vec(3+5*
i) < -
M_PI ) Y_vec(3+5*
i) +=
M_PI;
443 if ( Y_vec(3+5*
i) >
M_PI )
445 Y_vec(3+5*
i) = 2*
M_PI - Y_vec(3+5*
i);
446 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
448 if ( Y_vec(3+5*
i) < 0.0 )
450 Y_vec(3+5*
i) = - Y_vec(3+5*
i);
451 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
455 double SigE=0., SigPx=0., SigPy=0., SigPz=0., Px=0., Py=0., Pz=0.;
458 Amg::VectorX d0Cor(nTrk), d0Fac(nTrk), xcphiplusysphi(nTrk), xsphiminusycphi(nTrk);
459 d0Cor.setZero(); d0Fac.setZero(); xcphiplusysphi.setZero(); xsphiminusycphi.setZero();
461 conv_sign[0] = -1; conv_sign[1] = 1;
462 for (
unsigned int i=0;
i<nTrk; ++
i)
464 charge[
i] = (Y_vec(4+5*
i) < 0.) ? -1. : 1.;
465 rho[
i] =
sin(Y_vec(3+5*
i))/(B_z*Y_vec(4+5*
i));
466 xcphiplusysphi[
i] = A_vec(0)*
cos(Y_vec(2+5*
i))+A_vec(1)*
sin(Y_vec(2+5*
i));
467 xsphiminusycphi[
i] = A_vec(0)*
sin(Y_vec(2+5*
i))-A_vec(1)*
cos(Y_vec(2+5*
i));
468 if(fabs(-xcphiplusysphi[
i]/
rho[
i]) > 1.)
return nullptr;
469 d0Cor[
i] = 0.5*asin(-xcphiplusysphi[
i]/
rho[
i]);
470 double d0Facsq = 1. - xcphiplusysphi[
i]*xcphiplusysphi[
i]/(
rho[
i]*
rho[
i]);
471 d0Fac[
i] = (d0Facsq>0.) ? sqrt(d0Facsq) : 0;
472 Phi[
i] = Y_vec(2+5*
i) + 2.*d0Cor[
i];
485 double FMass=0., dFMassdxs=0., dFMassdys=0., dFMassdzs=0.;
486 double FPxy=0., dFPxydxs=0., dFPxydys=0., dFPxydzs=0., dFPxydxp=0., dFPxydyp=0., dFPxydzp=0.;
487 double FPxz=0., dFPxzdxs=0., dFPxzdys=0., dFPxzdzs=0., dFPxzdxp=0., dFPxzdyp=0., dFPxzdzp=0.;
489 Fxy.setZero(); Fxz.setZero(); dFMassdPhi.setZero();
490 Amg::VectorX drhodtheta(nTrk), drhodqOverP(nTrk), csplusbc(nTrk), ccminusbs(nTrk);
491 drhodtheta.setZero(); drhodqOverP.setZero(); csplusbc.setZero(); ccminusbs.setZero();
492 Amg::VectorX dFxydd0(nTrk), dFxydz0(nTrk), dFxydphi(nTrk), dFxydtheta(nTrk), dFxydqOverP(nTrk);
493 dFxydd0.setZero(); dFxydz0.setZero(); dFxydphi.setZero(); dFxydtheta.setZero(); dFxydqOverP.setZero();
494 Amg::VectorX dFxydxs(nTrk), dFxydys(nTrk), dFxydzs(nTrk);
495 dFxydxs.setZero(); dFxydys.setZero(); dFxydzs.setZero();
496 Amg::VectorX dFxzdd0(nTrk), dFxzdz0(nTrk), dFxzdphi(nTrk), dFxzdtheta(nTrk), dFxzdqOverP(nTrk);
497 dFxzdd0.setZero(); dFxzdz0.setZero(); dFxzdphi.setZero(); dFxzdtheta.setZero(); dFxzdqOverP.setZero();
498 Amg::VectorX dFxzdxs(nTrk), dFxzdys(nTrk), dFxzdzs(nTrk);
499 dFxzdxs.setZero(); dFxzdys.setZero(); dFxzdzs.setZero();
500 Amg::VectorX dFMassdd0(nTrk), dFMassdz0(nTrk), dFMassdphi(nTrk), dFMassdtheta(nTrk), dFMassdqOverP(nTrk);
501 dFMassdd0.setZero(); dFMassdz0.setZero(); dFMassdphi.setZero(); dFMassdtheta.setZero(); dFMassdqOverP.setZero();
502 Amg::VectorX dFPxydd0(nTrk), dFPxydz0(nTrk), dFPxydphi(nTrk), dFPxydtheta(nTrk), dFPxydqOverP(nTrk);
503 dFPxydd0.setZero(); dFPxydz0.setZero(); dFPxydphi.setZero(); dFPxydtheta.setZero(); dFPxydqOverP.setZero();
504 Amg::VectorX dFPxzdd0(nTrk), dFPxzdz0(nTrk), dFPxzdphi(nTrk), dFPxzdtheta(nTrk), dFPxzdqOverP(nTrk);
505 dFPxzdd0.setZero(); dFPxzdz0.setZero(); dFPxzdphi.setZero(); dFPxzdtheta.setZero(); dFPxzdqOverP.setZero();
506 Amg::VectorX dPhidd0(nTrk), dPhidz0(nTrk), dPhidphi0(nTrk), dPhidtheta(nTrk), dPhidqOverP(nTrk);
507 dPhidd0.setZero(); dPhidz0.setZero(); dPhidphi0.setZero(); dPhidtheta.setZero(); dPhidqOverP.setZero();
508 Amg::VectorX dPhidxs(nTrk), dPhidys(nTrk), dPhidzs(nTrk);
509 dPhidxs.setZero(); dPhidys.setZero(); dPhidzs.setZero();
518 FMass = constraintMass*constraintMass - SigE*SigE + SigPx*SigPx + SigPy*SigPy + SigPz*SigPz;
523 FPxy = Px*(frameOriginItr[1] - y_point) - Py*(frameOriginItr[0]- x_point);
527 FPxz = Px*(frameOriginItr[2] - z_point) - Pz*(frameOriginItr[0]- x_point);
529 for (
unsigned int i=0;
i<nTrk; ++
i)
534 Fxy[
i] = Y_vec(0+5*
i) + xsphiminusycphi[
i] - 2.*
rho[
i]*
sin(d0Cor[
i])*
sin(d0Cor[
i]);
538 Fxz[
i] = Y_vec(1+5*
i) - A_vec(2) -
rho[
i]*2.*d0Cor[
i]/
tan(Y_vec(3+5*
i));
542 drhodtheta[
i] =
cos(Y_vec(3+5*
i))/(B_z*Y_vec(4+5*
i));
543 drhodqOverP[
i] = -
sin(Y_vec(3+5*
i))/(B_z*Y_vec(4+5*
i)*Y_vec(4+5*
i));
546 dFxydphi[
i] = xcphiplusysphi[
i]*(1. + xsphiminusycphi[
i]/(d0Fac[
i]*
rho[
i]));
547 dFxydtheta[
i] = (xcphiplusysphi[
i]*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i])-2.*
sin(d0Cor[
i])*
sin(d0Cor[
i]))*drhodtheta[
i];
548 dFxydqOverP[
i] = (xcphiplusysphi[
i]*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i])-2.*
sin(d0Cor[
i])*
sin(d0Cor[
i]))*drhodqOverP[
i];
549 dFxydxs[
i] =
sin(Y_vec(2+5*
i)) -
cos(Y_vec(2+5*
i))*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]);
550 dFxydys[
i] = -
cos(Y_vec(2+5*
i)) -
sin(Y_vec(2+5*
i))*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]);
553 dFxzdphi[
i] = -xsphiminusycphi[
i]/(d0Fac[
i]*
tan(Y_vec(3+5*
i)));
554 dFxzdtheta[
i] = -((xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]) + 2.*d0Cor[
i])*
tan(Y_vec(3+5*
i))*drhodtheta[
i] -
556 dFxzdqOverP[
i] = -(xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]) + 2.*d0Cor[
i])*drhodqOverP[
i]/
tan(Y_vec(3+5*
i));
557 dFxzdxs[
i] =
cos(Y_vec(2+5*
i))/(d0Fac[
i]*
tan(Y_vec(3+5*
i)));
558 dFxzdys[
i] =
sin(Y_vec(2+5*
i))/(d0Fac[
i]*
tan(Y_vec(3+5*
i)));
561 dPhidphi0[
i] = 1. + xsphiminusycphi[
i]/(d0Fac[
i]*
rho[
i]);
562 dPhidtheta[
i] = xcphiplusysphi[
i]*drhodtheta[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i]);
563 dPhidqOverP[
i] = xcphiplusysphi[
i]*drhodqOverP[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i]);
564 dPhidxs[
i] = -
cos(Y_vec(2+5*
i))/(d0Fac[
i]*
rho[
i]);
565 dPhidys[
i] = -
sin(Y_vec(2+5*
i))/(d0Fac[
i]*
rho[
i]);
569 dFMassdphi[
i] = conv_sign[
i]*dPhidphi0[
i];
570 dFMassdtheta[
i] = conv_sign[
i]*dPhidtheta[
i];
571 dFMassdqOverP[
i] = conv_sign[
i]*dPhidqOverP[
i];
572 dFMassdxs += conv_sign[
i]*dPhidxs[
i];
573 dFMassdys += conv_sign[
i]*dPhidys[
i];
575 csplusbc[
i] = SigPy*
sin(Y_vec(2+5*
i))+SigPx*
cos(Y_vec(2+5*
i));
576 ccminusbs[
i] = SigPy*
cos(Y_vec(2+5*
i))-SigPx*
sin(Y_vec(2+5*
i));
577 dFMassdphi[
i] = 2.*
sin(Y_vec(3+5*
i))*ccminusbs[
i]*
charge[
i]/Y_vec(4+5*
i);
578 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);
579 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)) -
580 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));
584 if (pointingConstraint){
585 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);
586 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);
587 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));
593 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);
594 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)
595 +
sin(Y_vec(3+5*
i))*(frameOriginItr[0]-x_point)*
charge[
i]/Y_vec(4+5*
i);
596 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))
597 +
cos(Y_vec(3+5*
i))*(frameOriginItr[0]-x_point)*
charge[
i]/(Y_vec(4+5*
i)*Y_vec(4+5*
i));
606 F_vec[
i+nTrk] = -Fxz[
i];
608 F_fac_vec[
i+nTrk] = 1.;
610 if(massConstraint) F_vec(2*nTrk+0) = -FMass;
612 if(massConstraint) F_fac_vec(2*nTrk+0) = 0.000001;
613 if(pointingConstraint) {
615 F_vec(2*nTrk+1) = -FPxy;
616 F_vec(2*nTrk+2) = -FPxz;
617 F_fac_vec(2*nTrk+1) = 0.000001;
618 F_fac_vec(2*nTrk+2) = 0.000001;
620 F_vec(2*nTrk+0) = -FPxy;
621 F_vec(2*nTrk+1) = -FPxz;
622 F_fac_vec(2*nTrk+0) = 0.000001;
623 F_fac_vec(2*nTrk+1) = 0.000001;
628 for (
unsigned int i=0;
i<
dim; ++
i)
630 sumConstr += F_fac_vec[
i]*fabs(F_vec[
i]);
632 if ( std::isnan(sumConstr) ) {
return nullptr; }
633 if (sumConstr < 0.001) { onConstr =
true; }
636 for (
unsigned int i=0;
i<nTrk; ++
i)
638 Bjac_mat(
i,0+5*
i) = dFxydd0(
i);
639 Bjac_mat(
i,1+5*
i) = dFxydz0(
i);
640 Bjac_mat(
i,2+5*
i) = dFxydphi(
i);
641 Bjac_mat(
i,3+5*
i) = dFxydtheta(
i);
642 Bjac_mat(
i,4+5*
i) = dFxydqOverP(
i);
643 Bjac_mat(
i+nTrk,0+5*
i) = dFxzdd0(
i);
644 Bjac_mat(
i+nTrk,1+5*
i) = dFxzdz0(
i);
645 Bjac_mat(
i+nTrk,2+5*
i) = dFxzdphi(
i);
646 Bjac_mat(
i+nTrk,3+5*
i) = dFxzdtheta(
i);
647 Bjac_mat(
i+nTrk,4+5*
i) = dFxzdqOverP(
i);
649 Bjac_mat(2*nTrk,0+5*
i) = dFMassdd0(
i);
650 Bjac_mat(2*nTrk,1+5*
i) = dFMassdz0(
i);
651 Bjac_mat(2*nTrk,2+5*
i) = dFMassdphi(
i);
652 Bjac_mat(2*nTrk,3+5*
i) = dFMassdtheta(
i);
653 Bjac_mat(2*nTrk,4+5*
i) = dFMassdqOverP(
i);
655 if(pointingConstraint) {
657 Bjac_mat(2*nTrk+1,0+5*
i) = dFPxydd0(
i);
658 Bjac_mat(2*nTrk+1,1+5*
i) = dFPxydz0(
i);
659 Bjac_mat(2*nTrk+1,2+5*
i) = dFPxydphi(
i);
660 Bjac_mat(2*nTrk+1,3+5*
i) = dFPxydtheta(
i);
661 Bjac_mat(2*nTrk+1,4+5*
i) = dFPxydqOverP(
i);
662 Bjac_mat(2*nTrk+1,5*nTrk) = dFPxydxp;
663 Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxydyp;
664 Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxydzp;
665 Bjac_mat(2*nTrk+2,0+5*
i) = dFPxzdd0(
i);
666 Bjac_mat(2*nTrk+2,1+5*
i) = dFPxzdz0(
i);
667 Bjac_mat(2*nTrk+2,2+5*
i) = dFPxzdphi(
i);
668 Bjac_mat(2*nTrk+2,3+5*
i) = dFPxzdtheta(
i);
669 Bjac_mat(2*nTrk+2,4+5*
i) = dFPxzdqOverP(
i);
670 Bjac_mat(2*nTrk+2,5*nTrk) = dFPxzdxp;
671 Bjac_mat(2*nTrk+2,5*nTrk+1) = dFPxzdyp;
672 Bjac_mat(2*nTrk+2,5*nTrk+2) = dFPxzdzp;
674 Bjac_mat(2*nTrk+0,0+5*
i) = dFPxydd0(
i);
675 Bjac_mat(2*nTrk+0,1+5*
i) = dFPxydz0(
i);
676 Bjac_mat(2*nTrk+0,2+5*
i) = dFPxydphi(
i);
677 Bjac_mat(2*nTrk+0,3+5*
i) = dFPxydtheta(
i);
678 Bjac_mat(2*nTrk+0,4+5*
i) = dFPxydqOverP(
i);
679 Bjac_mat(2*nTrk+0,5*nTrk) = dFPxydxp;
680 Bjac_mat(2*nTrk+0,5*nTrk+1) = dFPxydyp;
681 Bjac_mat(2*nTrk+0,5*nTrk+2) = dFPxydzp;
682 Bjac_mat(2*nTrk+1,0+5*
i) = dFPxzdd0(
i);
683 Bjac_mat(2*nTrk+1,1+5*
i) = dFPxzdz0(
i);
684 Bjac_mat(2*nTrk+1,2+5*
i) = dFPxzdphi(
i);
685 Bjac_mat(2*nTrk+1,3+5*
i) = dFPxzdtheta(
i);
686 Bjac_mat(2*nTrk+1,4+5*
i) = dFPxzdqOverP(
i);
687 Bjac_mat(2*nTrk+1,5*nTrk) = dFPxzdxp;
688 Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxzdyp;
689 Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxzdzp;
693 Ajac_mat(
i,0) = dFxydxs(
i);
694 Ajac_mat(
i,1) = dFxydys(
i);
695 Ajac_mat(
i,2) = dFxydzs(
i);
696 Ajac_mat(
i+nTrk,0) = dFxzdxs(
i);
697 Ajac_mat(
i+nTrk,1) = dFxzdys(
i);
698 Ajac_mat(
i+nTrk,2) = dFxzdzs(
i);
700 Ajac_mat(2*nTrk,0) = dFMassdxs;
701 Ajac_mat(2*nTrk,1) = dFMassdys;
702 Ajac_mat(2*nTrk,2) = dFMassdzs;
704 if(pointingConstraint) {
706 Ajac_mat(2*nTrk+1,0) = dFPxydxs;
707 Ajac_mat(2*nTrk+1,1) = dFPxydys;
708 Ajac_mat(2*nTrk+1,2) = dFPxydzs;
709 Ajac_mat(2*nTrk+2,0) = dFPxzdxs;
710 Ajac_mat(2*nTrk+2,1) = dFPxzdys;
711 Ajac_mat(2*nTrk+2,2) = dFPxzdzs;
713 Ajac_mat(2*nTrk+0,0) = dFPxydxs;
714 Ajac_mat(2*nTrk+0,1) = dFPxydys;
715 Ajac_mat(2*nTrk+0,2) = dFPxydzs;
716 Ajac_mat(2*nTrk+1,0) = dFPxzdxs;
717 Ajac_mat(2*nTrk+1,1) = dFPxzdys;
718 Ajac_mat(2*nTrk+1,2) = dFPxzdzs;
723 Wb_mat = Wmeas_mat.similarity(Bjac_mat) ;
724 Wb_mat = Wb_mat.inverse();
726 C22_mat = Wb_mat.similarity(Ajac_mat.transpose());
727 C22_mat = C22_mat.inverse();
729 Btemp_mat = Wb_mat * Bjac_mat * Wmeas_mat;
730 Atemp_mat = Wb_mat * Ajac_mat;
732 C21_mat = - C22_mat * Ajac_mat.transpose() * Btemp_mat;
733 C32_mat = Atemp_mat * C22_mat;
734 C31_mat = Btemp_mat + Atemp_mat * C21_mat;
735 Amg::MatrixX mat_prod_1 = Wmeas_mat * Bjac_mat.transpose();
736 Amg::MatrixX mat_prod_2 = Wmeas_mat * Bjac_mat.transpose() * Wb_mat * Ajac_mat;
737 C11_mat = Wmeas_mat - Wb_mat.similarity( mat_prod_1 ) + C22_mat.similarity( mat_prod_2 );
739 C_cor_vec = Ajac_mat*DeltaA_vec + Bjac_mat*DeltaY_vec;
740 C_vec = C_cor_vec + F_vec;
742 DeltaY_vec = C31_mat.transpose()*C_vec;
743 DeltaA_vec = C32_mat.transpose()*C_vec;
745 for (
unsigned int i=0;
i<n_dim; ++
i)
747 ChiItr_vec(0,
i) = DeltaY_vec(
i);
749 ChiItr_mat = Wmeas0_mat.similarity( ChiItr_vec );
750 chi2New = ChiItr_mat(0,0);
753 frameOriginItr[0] += DeltaA_vec(0);
754 frameOriginItr[1] += DeltaA_vec(1);
755 frameOriginItr[2] += DeltaA_vec(2);
757 msg(
MSG::DEBUG) <<
"New vertex, global coordinates: " << frameOriginItr.transpose() <<
endmsg;
758 msg(
MSG::DEBUG) <<
"chi2Old: " << chi2Old <<
" chi2New: " << chi2New <<
" fabs(chi2Old-chi2New): " << fabs(chi2Old-chi2New) <<
endmsg;
762 if (globalPositionItr->perp() >
m_maxR && globalPositionItr->z() >
m_maxZ)
return nullptr;
764 if (onConstr && fabs(chi2Old-chi2New) < 0.1) {
break; }
767 fieldCache.
getField(globalPositionItr->data(),BFieldItr);
768 double B_z_new = BFieldItr[2]*299.792;
769 if (B_z_new == 0. || std::isnan(B_z)) {
775 double deltaR = sqrt(DeltaA_vec(0)*DeltaA_vec(0)+DeltaA_vec(1)*DeltaA_vec(1)+DeltaA_vec(2)*DeltaA_vec(2));
776 double deltaB_z = fabs(B_z-B_z_new)/B_z;
777 bool changeBz =
false;
788 v0FitterTracks.clear();
793 if (chargeParameters !=
nullptr)
796 const Amg::Vector3D gMomentum = chargeParameters->momentum();
797 const Amg::Vector3D gDirection = chargeParameters->position() - *globalPositionItr;
798 const double extrapolationDirection = gMomentum .dot( gDirection );
801 std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(
nullptr);
802 std::unique_ptr<const Trk::TrackParameters>
tmp =
812 extrapolatedPerigee.reset(
816 if (extrapolatedPerigee ==
nullptr) {
817 ATH_MSG_DEBUG(
"Perigee was not extrapolated! Taking original one!");
819 if (tmpPerigee!=
nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
824 V0FitterTrack locV0FitterTrack;
825 locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[
Trk::d0];
826 locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[
Trk::z0];
827 locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[
Trk::phi];
828 locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[
Trk::theta];
829 locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[
Trk::qOverP];
830 locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
831 locV0FitterTrack.originalPerigee = chargeParameters;
832 v0FitterTracks.push_back(locV0FitterTrack);
834 ATH_MSG_DEBUG(
"Track parameters are not charged tracks ... fit aborted");
838 frameOrigin = frameOriginItr;
844 chi2Old = 2000000000000.;
855 frameOrigin[0] += DeltaA_vec(0);
856 frameOrigin[1] += DeltaA_vec(1);
857 frameOrigin[2] += DeltaA_vec(2);
858 if ( std::isnan(frameOrigin[0]) || std::isnan(frameOrigin[1]) || std::isnan(frameOrigin[2]) )
return nullptr;
860 Y_vec = Y0_vec + DeltaY_vec;
863 for (
unsigned int i=0;
i<nTrk; ++
i)
865 if ( fabs ( Y_vec(2+5*
i) ) > 100. || fabs ( Y_vec(3+5*
i) ) > 100. ) {
return nullptr; }
866 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;
867 while ( Y_vec(3+5*
i) > 2*
M_PI ) Y_vec(3+5*
i) -= 2*
M_PI;
868 while ( Y_vec(3+5*
i) < -
M_PI ) Y_vec(3+5*
i) +=
M_PI;
869 if ( Y_vec(3+5*
i) >
M_PI )
871 Y_vec(3+5*
i) = 2*
M_PI - Y_vec(3+5*
i);
872 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
874 if ( Y_vec(3+5*
i) < 0.0 )
876 Y_vec(3+5*
i) = - Y_vec(3+5*
i);
877 if ( Y_vec(2+5*
i) >= 0 ) Y_vec(2+5*
i) += ( Y_vec(2+5*
i) >0 ) ? -
M_PI :
M_PI;
881 for (
unsigned int i=0;
i<n_dim; ++
i)
883 Chi_vec(0,
i) = DeltaY_vec(
i);
885 Chi_mat = Wmeas0_mat.similarity( Chi_vec );
889 V_mat.block(0,0,n_dim,n_dim) = C11_mat;
890 V_mat.block(n_dim,n_dim,3,3) = C22_mat;
891 V_mat.block(n_dim,0,3,n_dim) = C21_mat;
892 V_mat.block(0,n_dim,n_dim,3) = C21_mat.transpose();
897 for (BTIter = v0FitterTracks.begin(); BTIter != v0FitterTracks.end() ; ++BTIter)
901 covTrk = Wmeas0_mat.block(5*iRP,5*iRP,4+5*iRP,4+5*iRP);
903 for (
unsigned int i=0;
i<5; ++
i) chi_vec(
i) = DeltaY_vec(
i+5*iRP);
904 double chi2Trk = chi_vec.
dot(covTrk*chi_vec);
905 (*BTIter).
chi2=chi2Trk;
911 vx->makePrivateStore();
912 vx->setPosition (frameOrigin);
913 vx->setCovariancePosition (C22_mat);
914 vx->setFitQuality(
chi2,static_cast<
float>(
ndf));
918 std::
vector<VxTrackAtVertex> & tracksAtVertex = vx->vxTrackAtVertex(); tracksAtVertex.
clear();
922 unsigned int iterf=0;
924 for (BTIterf = v0FitterTracks.
begin(); BTIterf != v0FitterTracks.
end() ; ++BTIterf)
927 CovMtxP.setIdentity();
928 for (
unsigned int i=0;
i<5; ++
i) {
929 for (
unsigned int j=0; j<
i+1; ++j) {
930 double val = V_mat(5*iterf+
i,5*iterf+j);
931 CovMtxP.fillSymmetric(
i,j,
val);
934 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),
936 tracksAtVertex.emplace_back((*BTIterf).chi2, refittedPerigee, (*BTIterf).originalPerigee);
941 unsigned int sfcmv = nPar*(nPar+1)/2;
942 std::vector<float> floatErrMtx(sfcmv,0.);
943 unsigned int ipnt = 0;
944 for (
unsigned int i=0;
i<nPar; ++
i) {
945 for (
unsigned int j=0; j<
i+1; ++j) {
946 floatErrMtx[ipnt++]=V_mat(
i,j);
949 vx->setCovariance(floatErrMtx);