237 {
238 if ( originalPerigees.empty() )
239 {
241 return nullptr;
242 }
243
244
245 bool pointingConstraint = false;
246 bool massConstraint = false;
247 if(constraintMass > -100.) massConstraint = true;
248 bool conversion = false;
249 if(constraintMass == 0. && originalPerigees.size() == 2) conversion = true;
250 double x_point=0., y_point=0., z_point=0.;
251 AmgSymMatrix(3) pointingVertexCov; pointingVertexCov.setIdentity();
252 if (pointingVertex !=
nullptr) {
253 if (pointingVertex->covariancePosition().trace() != 0.) {
254 pointingConstraint = true;
255 Amg::Vector3D pv = pointingVertex->position();
256 x_point = pv.x();
257 y_point = pv.y();
258 z_point = pv.z();
259 pointingVertexCov = pointingVertex->covariancePosition().inverse();
260 }
261 }
262
263 if (msgLvl(MSG::DEBUG)) {
264 msg(MSG::DEBUG) <<
"massConstraint " << massConstraint <<
" pointingConstraint " << pointingConstraint <<
" conversion " << conversion <<
endmsg;
265 msg(MSG::DEBUG) <<
"V0Fitter called with: " <<
endmsg;
266 if (massConstraint && !
masses.empty())
msg(MSG::DEBUG) <<
"mass constraint, V0Mass = " << constraintMass <<
" particle masses " <<
masses <<
endmsg;
267 if (pointingConstraint)
msg(MSG::DEBUG) <<
"pointing constraint, x = " << x_point <<
" y = " << y_point <<
" z = " << z_point <<
endmsg;
268 }
269
270 bool restartFit = true;
271 double chi2 = 2000000000000.;
272 unsigned int nTrk = originalPerigees.size();
273 unsigned int nMeas = 5*nTrk;
274 unsigned int nVert = 1;
275
276 unsigned int nCnst = 2*nTrk;
277 unsigned int nPntC = 2;
278 unsigned int nMass = 1;
279
280 if (massConstraint) {
281 nCnst = nCnst + nMass;
282 }
283 if (pointingConstraint) {
284 nCnst = nCnst + nPntC;
285 nMeas = nMeas + 3;
286 nVert = nVert + 1;
287 }
288
289 unsigned int nPar = 5*nTrk + 3*nVert;
290 int ndf = nMeas - (nPar - nCnst);
291 if (ndf < 0) {
ndf = 1;}
292
293 unsigned int dim = nCnst;
294 unsigned int n_dim = nMeas;
295
296 ATH_MSG_DEBUG(
"ndf " << ndf <<
" n_dim " << n_dim <<
" dim " << dim);
297
298 std::vector<V0FitterTrack> v0FitterTracks;
299
303
304 Amg::MatrixX Wmeas_mat(n_dim,n_dim); Wmeas_mat.setZero();
305 Amg::MatrixX Wmeas0_mat(n_dim,n_dim); Wmeas0_mat.setZero();
325 Amg::MatrixX ChiItr_vec(1,n_dim); ChiItr_vec.setZero();
327 Amg::VectorX F_fac_vec(dim); F_fac_vec.setZero();
328
329 const Amg::
Vector3D * globalPosition = &(firstStartingPoint);
330 ATH_MSG_DEBUG(
"globalPosition of starting point: " << (*globalPosition)[0] <<
", " << (*globalPosition)[1] <<
", " << (*globalPosition)[2]);
331
333
335 if (!readHandle.isValid()) {
336 std::string msg = "Failed to retrieve magmnetic field conditions data ";
337 msg += m_fieldCacheCondObjInputKey.key();
338 throw std::runtime_error(msg);
339 }
340 const AtlasFieldCacheCondObj* fieldCondObj{*readHandle};
343 return nullptr;
344 }
345 MagField::AtlasFieldCache fieldCache;
346 fieldCondObj->getInitializedCache (fieldCache);
347
348
349 double BField[3];
350 fieldCache.
getField(globalPosition->data(),BField);
351 double B_z = BField[2]*299.792;
352 if (B_z == 0. || std::isnan(B_z)) {
353 ATH_MSG_DEBUG(
"Could not find a magnetic field different from zero: very very strange");
354 B_z = 0.60407;
355 } else {
356 ATH_MSG_VERBOSE(
"Magnetic field projection of z axis in the perigee position is: " << B_z <<
" GeV/mm ");
357 }
358
359
360
361 v0FitterTracks.clear();
362 Trk::PerigeeSurface perigeeSurface(*globalPosition);
363
365 {
366 if (chargeParameters != nullptr)
367 {
368
369 const Amg::Vector3D gMomentum = chargeParameters->momentum();
370 const Amg::Vector3D gDirection = chargeParameters->position() - *globalPosition;
371 const double extrapolationDirection = gMomentum.dot( gDirection );
374 std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(nullptr);
375
376 std::unique_ptr<const Trk::TrackParameters>
tmp =
377 std::abs(chargeParameters->position().z()) >
m_maxZ ? nullptr :
379 *chargeParameters,
380 perigeeSurface,
382 true,
384 mode);
385
386
388 extrapolatedPerigee.reset(
static_cast<const Trk::Perigee*
>(
tmp.release()));
389 }
390
391 if (extrapolatedPerigee == nullptr) {
392 ATH_MSG_DEBUG(
"Perigee was not extrapolated! Taking original one!");
394 if (tmpPerigee!=nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
395 else return nullptr;
396 }
397
398
399 V0FitterTrack locV0FitterTrack{};
400 locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[
Trk::d0];
401 locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[
Trk::z0];
402 locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[
Trk::phi];
403 locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[
Trk::theta];
404 locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[
Trk::qOverP];
405 locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
406 locV0FitterTrack.originalPerigee = chargeParameters;
407 v0FitterTracks.push_back(locV0FitterTrack);
408 } else {
409 ATH_MSG_DEBUG(
"Track parameters are not charged tracks ... fit aborted");
410 return nullptr;
411 }
412 }
413
414
415 double chi2New=0.;
double chi2Old=
chi2;
416 double sumConstr=0.;
417 bool onConstr = false;
421 {
423 if (!restartFit) chi2Old = chi2New;
424 chi2New = 0.;
425
426 if (restartFit)
427 {
428
429 std::vector<V0FitterTrack>::iterator PTIter;
431 for (PTIter = v0FitterTracks.begin(); PTIter != v0FitterTracks.end() ; ++PTIter)
432 {
433 V0FitterTrack locP((*PTIter));
434 Wmeas0_mat.block<5,5>(5*
i,5*
i) = locP.Wi_mat;
435 Wmeas_mat.block<5,5>(5*
i,5*
i) = locP.Wi_mat;
436 for (
int j=0;
j<5; ++
j) {
437 Y0_vec(j+5*i) = locP.TrkPar[
j];
438 }
440 }
441 if(pointingConstraint) {
442 Y0_vec(5*nTrk + 0) = x_point;
443 Y0_vec(5*nTrk + 1) = y_point;
444 Y0_vec(5*nTrk + 2) = z_point;
445 Wmeas0_mat.block<3,3>(5*nTrk,5*nTrk) = pointingVertexCov;
446 Wmeas_mat.block<3,3>(5*nTrk,5*nTrk) = pointingVertexCov;
447 }
448 Wmeas_mat = Wmeas_mat.inverse();
449 }
450
451 Y_vec = Y0_vec + DeltaY_vec;
452 A_vec = DeltaA_vec;
453
454
455 for (
unsigned int i=0;
i<nTrk; ++
i)
456 {
457 if ( fabs ( Y_vec(2+5*i) ) > 100. || fabs ( Y_vec(3+5*i) ) > 100. ) { return nullptr; }
458 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;
459 while ( Y_vec(3+5*i) > 2*
M_PI ) Y_vec(3+5*i) -= 2*
M_PI;
460 while ( Y_vec(3+5*i) < -
M_PI ) Y_vec(3+5*i) +=
M_PI;
461 if ( Y_vec(3+5*i) >
M_PI )
462 {
463 Y_vec(3+5*i) = 2*
M_PI - Y_vec(3+5*i);
464 if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -
M_PI :
M_PI;
465 }
466 if ( Y_vec(3+5*i) < 0.0 )
467 {
468 Y_vec(3+5*i) = - Y_vec(3+5*i);
469 if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -
M_PI :
M_PI;
470 }
471 }
472
473 double SigE=0., SigPx=0., SigPy=0., SigPz=0., Px=0., Py=0., Pz=0.;
476 Amg::VectorX d0Cor(nTrk), d0Fac(nTrk), xcphiplusysphi(nTrk), xsphiminusycphi(nTrk);
477 d0Cor.setZero(); d0Fac.setZero(); xcphiplusysphi.setZero(); xsphiminusycphi.setZero();
479 conv_sign[0] = -1; conv_sign[1] = 1;
480 for (unsigned int i=0; i<nTrk; ++i)
481 {
482 charge[
i] = (Y_vec(4+5*i) < 0.) ? -1. : 1.;
483 rho[
i] =
sin(Y_vec(3+5*i))/(B_z*Y_vec(4+5*i));
484 xcphiplusysphi[
i] = A_vec(0)*
cos(Y_vec(2+5*i))+A_vec(1)*
sin(Y_vec(2+5*i));
485 xsphiminusycphi[
i] = A_vec(0)*
sin(Y_vec(2+5*i))-A_vec(1)*
cos(Y_vec(2+5*i));
486 if(fabs(-xcphiplusysphi[i]/rho[i]) > 1.) return nullptr;
487 d0Cor[
i] = 0.5*asin(-xcphiplusysphi[i]/rho[i]);
488 double d0Facsq = 1. - xcphiplusysphi[
i]*xcphiplusysphi[
i]/(
rho[
i]*
rho[
i]);
489 d0Fac[
i] = (d0Facsq>0.) ? sqrt(d0Facsq) : 0;
490 Phi[
i] = Y_vec(2+5*i) + 2.*d0Cor[
i];
491
492 if(massConstraint && !
masses.empty() && masses[i] != 0.){
493 SigE += sqrt(1./(Y_vec(4+5*i)*Y_vec(4+5*i)) + masses[i]*masses[i]);
494 SigPx +=
sin(Y_vec(3+5*i))*
cos(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
495 SigPy +=
sin(Y_vec(3+5*i))*
sin(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
496 SigPz +=
cos(Y_vec(3+5*i))*
charge[
i]/Y_vec(4+5*i);
497 }
498 Px +=
sin(Y_vec(3+5*i))*
cos(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
499 Py +=
sin(Y_vec(3+5*i))*
sin(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
500 Pz +=
cos(Y_vec(3+5*i))*
charge[
i]/Y_vec(4+5*i);
501 }
502
503 double FMass=0., dFMassdxs=0., dFMassdys=0., dFMassdzs=0.;
504 double FPxy=0., dFPxydxs=0., dFPxydys=0., dFPxydzs=0., dFPxydxp=0., dFPxydyp=0., dFPxydzp=0.;
505 double FPxz=0., dFPxzdxs=0., dFPxzdys=0., dFPxzdzs=0., dFPxzdxp=0., dFPxzdyp=0., dFPxzdzp=0.;
507 Fxy.setZero(); Fxz.setZero(); dFMassdPhi.setZero();
508 Amg::VectorX drhodtheta(nTrk), drhodqOverP(nTrk), csplusbc(nTrk), ccminusbs(nTrk);
509 drhodtheta.setZero(); drhodqOverP.setZero(); csplusbc.setZero(); ccminusbs.setZero();
510 Amg::VectorX dFxydd0(nTrk), dFxydz0(nTrk), dFxydphi(nTrk), dFxydtheta(nTrk), dFxydqOverP(nTrk);
511 dFxydd0.setZero(); dFxydz0.setZero(); dFxydphi.setZero(); dFxydtheta.setZero(); dFxydqOverP.setZero();
512 Amg::VectorX dFxydxs(nTrk), dFxydys(nTrk), dFxydzs(nTrk);
513 dFxydxs.setZero(); dFxydys.setZero(); dFxydzs.setZero();
514 Amg::VectorX dFxzdd0(nTrk), dFxzdz0(nTrk), dFxzdphi(nTrk), dFxzdtheta(nTrk), dFxzdqOverP(nTrk);
515 dFxzdd0.setZero(); dFxzdz0.setZero(); dFxzdphi.setZero(); dFxzdtheta.setZero(); dFxzdqOverP.setZero();
516 Amg::VectorX dFxzdxs(nTrk), dFxzdys(nTrk), dFxzdzs(nTrk);
517 dFxzdxs.setZero(); dFxzdys.setZero(); dFxzdzs.setZero();
518 Amg::VectorX dFMassdd0(nTrk), dFMassdz0(nTrk), dFMassdphi(nTrk), dFMassdtheta(nTrk), dFMassdqOverP(nTrk);
519 dFMassdd0.setZero(); dFMassdz0.setZero(); dFMassdphi.setZero(); dFMassdtheta.setZero(); dFMassdqOverP.setZero();
520 Amg::VectorX dFPxydd0(nTrk), dFPxydz0(nTrk), dFPxydphi(nTrk), dFPxydtheta(nTrk), dFPxydqOverP(nTrk);
521 dFPxydd0.setZero(); dFPxydz0.setZero(); dFPxydphi.setZero(); dFPxydtheta.setZero(); dFPxydqOverP.setZero();
522 Amg::VectorX dFPxzdd0(nTrk), dFPxzdz0(nTrk), dFPxzdphi(nTrk), dFPxzdtheta(nTrk), dFPxzdqOverP(nTrk);
523 dFPxzdd0.setZero(); dFPxzdz0.setZero(); dFPxzdphi.setZero(); dFPxzdtheta.setZero(); dFPxzdqOverP.setZero();
524 Amg::VectorX dPhidd0(nTrk), dPhidz0(nTrk), dPhidphi0(nTrk), dPhidtheta(nTrk), dPhidqOverP(nTrk);
525 dPhidd0.setZero(); dPhidz0.setZero(); dPhidphi0.setZero(); dPhidtheta.setZero(); dPhidqOverP.setZero();
526 Amg::VectorX dPhidxs(nTrk), dPhidys(nTrk), dPhidzs(nTrk);
527 dPhidxs.setZero(); dPhidys.setZero(); dPhidzs.setZero();
528
529
530
531
532
533 if (conversion) {
535 } else {
536 FMass = constraintMass*constraintMass - SigE*SigE + SigPx*SigPx + SigPy*SigPy + SigPz*SigPz;
537 }
538
539
540
541 FPxy = Px*(frameOriginItr[1] - y_point) - Py*(frameOriginItr[0]- x_point);
542
543
544
545 FPxz = Px*(frameOriginItr[2] - z_point) - Pz*(frameOriginItr[0]- x_point);
546
547 for (
unsigned int i=0;
i<nTrk; ++
i)
548 {
549
550
551
552 Fxy[
i] = Y_vec(0+5*i) + xsphiminusycphi[
i] - 2.*
rho[
i]*
sin(d0Cor[i])*
sin(d0Cor[i]);
553
554
555
556 Fxz[
i] = Y_vec(1+5*i) - A_vec(2) -
rho[
i]*2.*d0Cor[
i]/
tan(Y_vec(3+5*i));
557
558
559
560 drhodtheta[
i] =
cos(Y_vec(3+5*i))/(B_z*Y_vec(4+5*i));
561 drhodqOverP[
i] = -
sin(Y_vec(3+5*i))/(B_z*Y_vec(4+5*i)*Y_vec(4+5*i));
562
564 dFxydphi[
i] = xcphiplusysphi[
i]*(1. + xsphiminusycphi[
i]/(d0Fac[
i]*
rho[
i]));
565 dFxydtheta[
i] = (xcphiplusysphi[
i]*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i])-2.*
sin(d0Cor[i])*
sin(d0Cor[i]))*drhodtheta[i];
566 dFxydqOverP[
i] = (xcphiplusysphi[
i]*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i])-2.*
sin(d0Cor[i])*
sin(d0Cor[i]))*drhodqOverP[i];
567 dFxydxs[
i] =
sin(Y_vec(2+5*i)) -
cos(Y_vec(2+5*i))*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]);
568 dFxydys[
i] = -
cos(Y_vec(2+5*i)) -
sin(Y_vec(2+5*i))*xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]);
569
571 dFxzdphi[
i] = -xsphiminusycphi[
i]/(d0Fac[
i]*
tan(Y_vec(3+5*i)));
572 dFxzdtheta[
i] = -((xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]) + 2.*d0Cor[
i])*
tan(Y_vec(3+5*i))*drhodtheta[
i] -
573 rho[
i]*2.*d0Cor[
i]/(
cos(Y_vec(3+5*i))*
cos(Y_vec(3+5*i))))/(
tan(Y_vec(3+5*i))*
tan(Y_vec(3+5*i)));
574 dFxzdqOverP[
i] = -(xcphiplusysphi[
i]/(d0Fac[
i]*
rho[
i]) + 2.*d0Cor[
i])*drhodqOverP[i]/
tan(Y_vec(3+5*i));
575 dFxzdxs[
i] =
cos(Y_vec(2+5*i))/(d0Fac[
i]*
tan(Y_vec(3+5*i)));
576 dFxzdys[
i] =
sin(Y_vec(2+5*i))/(d0Fac[
i]*
tan(Y_vec(3+5*i)));
578
579 dPhidphi0[
i] = 1. + xsphiminusycphi[
i]/(d0Fac[
i]*
rho[
i]);
580 dPhidtheta[
i] = xcphiplusysphi[
i]*drhodtheta[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i]);
581 dPhidqOverP[
i] = xcphiplusysphi[
i]*drhodqOverP[
i]/(d0Fac[
i]*
rho[
i]*
rho[
i]);
582 dPhidxs[
i] = -
cos(Y_vec(2+5*i))/(d0Fac[
i]*
rho[
i]);
583 dPhidys[
i] = -
sin(Y_vec(2+5*i))/(d0Fac[
i]*
rho[
i]);
584
585 if (massConstraint && !
masses.empty() && masses[i] != 0.){
586 if (conversion) {
587 dFMassdphi[
i] = conv_sign[
i]*dPhidphi0[
i];
588 dFMassdtheta[
i] = conv_sign[
i]*dPhidtheta[
i];
589 dFMassdqOverP[
i] = conv_sign[
i]*dPhidqOverP[
i];
590 dFMassdxs += conv_sign[
i]*dPhidxs[
i];
591 dFMassdys += conv_sign[
i]*dPhidys[
i];
592 } else {
593 csplusbc[
i] = SigPy*
sin(Y_vec(2+5*i))+SigPx*
cos(Y_vec(2+5*i));
594 ccminusbs[
i] = SigPy*
cos(Y_vec(2+5*i))-SigPx*
sin(Y_vec(2+5*i));
595 dFMassdphi[
i] = 2.*
sin(Y_vec(3+5*i))*ccminusbs[
i]*
charge[
i]/Y_vec(4+5*i);
596 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);
597 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)) -
598 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));
599 }
600 }
601
602 if (pointingConstraint){
603 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);
604 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);
605 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));
606 dFPxydxs += -
sin(Y_vec(3+5*i))*
sin(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
607 dFPxydys +=
sin(Y_vec(3+5*i))*
cos(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
608 dFPxydxp +=
sin(Y_vec(3+5*i))*
sin(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
609 dFPxydyp += -
sin(Y_vec(3+5*i))*
cos(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
610
611 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);
612 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)
613 +
sin(Y_vec(3+5*i))*(frameOriginItr[0]-x_point)*
charge[i]/Y_vec(4+5*i);
614 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))
615 +
cos(Y_vec(3+5*i))*(frameOriginItr[0]-x_point)*
charge[i]/(Y_vec(4+5*i)*Y_vec(4+5*i));
616 dFPxzdxs += -
cos(Y_vec(3+5*i))*
charge[
i]/Y_vec(4+5*i);
617 dFPxzdzs +=
sin(Y_vec(3+5*i))*
cos(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
618 dFPxzdxp +=
cos(Y_vec(3+5*i))*
charge[
i]/Y_vec(4+5*i);
619 dFPxzdzp += -
sin(Y_vec(3+5*i))*
cos(Y_vec(2+5*i))*
charge[
i]/Y_vec(4+5*i);
620 }
621
622
624 F_vec[
i+nTrk] = -Fxz[
i];
626 F_fac_vec[
i+nTrk] = 1.;
627 }
628 if(massConstraint) F_vec(2*nTrk+0) = -FMass;
629
630 if(massConstraint) F_fac_vec(2*nTrk+0) = 0.000001;
631 if(pointingConstraint) {
632 if(massConstraint) {
633 F_vec(2*nTrk+1) = -FPxy;
634 F_vec(2*nTrk+2) = -FPxz;
635 F_fac_vec(2*nTrk+1) = 0.000001;
636 F_fac_vec(2*nTrk+2) = 0.000001;
637 } else {
638 F_vec(2*nTrk+0) = -FPxy;
639 F_vec(2*nTrk+1) = -FPxz;
640 F_fac_vec(2*nTrk+0) = 0.000001;
641 F_fac_vec(2*nTrk+1) = 0.000001;
642 }
643 }
644
645 sumConstr = 0.;
646 for (
unsigned int i=0;
i<
dim; ++
i)
647 {
648 sumConstr += F_fac_vec[
i]*fabs(F_vec[i]);
649 }
650 if ( std::isnan(sumConstr) ) { return nullptr; }
651 if (sumConstr < 0.001) { onConstr = true; }
653
654 for (
unsigned int i=0;
i<nTrk; ++
i)
655 {
656 Bjac_mat(i,0+5*i) = dFxydd0(i);
657 Bjac_mat(i,1+5*i) = dFxydz0(i);
658 Bjac_mat(i,2+5*i) = dFxydphi(i);
659 Bjac_mat(i,3+5*i) = dFxydtheta(i);
660 Bjac_mat(i,4+5*i) = dFxydqOverP(i);
661 Bjac_mat(i+nTrk,0+5*i) = dFxzdd0(i);
662 Bjac_mat(i+nTrk,1+5*i) = dFxzdz0(i);
663 Bjac_mat(i+nTrk,2+5*i) = dFxzdphi(i);
664 Bjac_mat(i+nTrk,3+5*i) = dFxzdtheta(i);
665 Bjac_mat(i+nTrk,4+5*i) = dFxzdqOverP(i);
666 if(massConstraint) {
667 Bjac_mat(2*nTrk,0+5*i) = dFMassdd0(i);
668 Bjac_mat(2*nTrk,1+5*i) = dFMassdz0(i);
669 Bjac_mat(2*nTrk,2+5*i) = dFMassdphi(i);
670 Bjac_mat(2*nTrk,3+5*i) = dFMassdtheta(i);
671 Bjac_mat(2*nTrk,4+5*i) = dFMassdqOverP(i);
672 }
673 if(pointingConstraint) {
674 if(massConstraint) {
675 Bjac_mat(2*nTrk+1,0+5*i) = dFPxydd0(i);
676 Bjac_mat(2*nTrk+1,1+5*i) = dFPxydz0(i);
677 Bjac_mat(2*nTrk+1,2+5*i) = dFPxydphi(i);
678 Bjac_mat(2*nTrk+1,3+5*i) = dFPxydtheta(i);
679 Bjac_mat(2*nTrk+1,4+5*i) = dFPxydqOverP(i);
680 Bjac_mat(2*nTrk+1,5*nTrk) = dFPxydxp;
681 Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxydyp;
682 Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxydzp;
683 Bjac_mat(2*nTrk+2,0+5*i) = dFPxzdd0(i);
684 Bjac_mat(2*nTrk+2,1+5*i) = dFPxzdz0(i);
685 Bjac_mat(2*nTrk+2,2+5*i) = dFPxzdphi(i);
686 Bjac_mat(2*nTrk+2,3+5*i) = dFPxzdtheta(i);
687 Bjac_mat(2*nTrk+2,4+5*i) = dFPxzdqOverP(i);
688 Bjac_mat(2*nTrk+2,5*nTrk) = dFPxzdxp;
689 Bjac_mat(2*nTrk+2,5*nTrk+1) = dFPxzdyp;
690 Bjac_mat(2*nTrk+2,5*nTrk+2) = dFPxzdzp;
691 } else {
692 Bjac_mat(2*nTrk+0,0+5*i) = dFPxydd0(i);
693 Bjac_mat(2*nTrk+0,1+5*i) = dFPxydz0(i);
694 Bjac_mat(2*nTrk+0,2+5*i) = dFPxydphi(i);
695 Bjac_mat(2*nTrk+0,3+5*i) = dFPxydtheta(i);
696 Bjac_mat(2*nTrk+0,4+5*i) = dFPxydqOverP(i);
697 Bjac_mat(2*nTrk+0,5*nTrk) = dFPxydxp;
698 Bjac_mat(2*nTrk+0,5*nTrk+1) = dFPxydyp;
699 Bjac_mat(2*nTrk+0,5*nTrk+2) = dFPxydzp;
700 Bjac_mat(2*nTrk+1,0+5*i) = dFPxzdd0(i);
701 Bjac_mat(2*nTrk+1,1+5*i) = dFPxzdz0(i);
702 Bjac_mat(2*nTrk+1,2+5*i) = dFPxzdphi(i);
703 Bjac_mat(2*nTrk+1,3+5*i) = dFPxzdtheta(i);
704 Bjac_mat(2*nTrk+1,4+5*i) = dFPxzdqOverP(i);
705 Bjac_mat(2*nTrk+1,5*nTrk) = dFPxzdxp;
706 Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxzdyp;
707 Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxzdzp;
708 }
709 }
710
711 Ajac_mat(i,0) = dFxydxs(i);
712 Ajac_mat(i,1) = dFxydys(i);
713 Ajac_mat(i,2) = dFxydzs(i);
714 Ajac_mat(i+nTrk,0) = dFxzdxs(i);
715 Ajac_mat(i+nTrk,1) = dFxzdys(i);
716 Ajac_mat(i+nTrk,2) = dFxzdzs(i);
717 if(massConstraint) {
718 Ajac_mat(2*nTrk,0) = dFMassdxs;
719 Ajac_mat(2*nTrk,1) = dFMassdys;
720 Ajac_mat(2*nTrk,2) = dFMassdzs;
721 }
722 if(pointingConstraint) {
723 if(massConstraint) {
724 Ajac_mat(2*nTrk+1,0) = dFPxydxs;
725 Ajac_mat(2*nTrk+1,1) = dFPxydys;
726 Ajac_mat(2*nTrk+1,2) = dFPxydzs;
727 Ajac_mat(2*nTrk+2,0) = dFPxzdxs;
728 Ajac_mat(2*nTrk+2,1) = dFPxzdys;
729 Ajac_mat(2*nTrk+2,2) = dFPxzdzs;
730 } else {
731 Ajac_mat(2*nTrk+0,0) = dFPxydxs;
732 Ajac_mat(2*nTrk+0,1) = dFPxydys;
733 Ajac_mat(2*nTrk+0,2) = dFPxydzs;
734 Ajac_mat(2*nTrk+1,0) = dFPxzdxs;
735 Ajac_mat(2*nTrk+1,1) = dFPxzdys;
736 Ajac_mat(2*nTrk+1,2) = dFPxzdzs;
737 }
738 }
739 }
740
741 Wb_mat = Wmeas_mat.similarity(Bjac_mat) ;
742 Wb_mat = Wb_mat.inverse();
743
744 C22_mat = Wb_mat.similarity(Ajac_mat.transpose());
745 C22_mat = C22_mat.inverse();
746
747 Btemp_mat = Wb_mat * Bjac_mat * Wmeas_mat;
748 Atemp_mat = Wb_mat * Ajac_mat;
749
750 C21_mat = - C22_mat * Ajac_mat.transpose() * Btemp_mat;
751 C32_mat = Atemp_mat * C22_mat;
752 C31_mat = Btemp_mat + Atemp_mat * C21_mat;
753 Amg::MatrixX mat_prod_1 = Wmeas_mat * Bjac_mat.transpose();
754 Amg::MatrixX mat_prod_2 = Wmeas_mat * Bjac_mat.transpose() * Wb_mat * Ajac_mat;
755 C11_mat = Wmeas_mat - Wb_mat.similarity( mat_prod_1 ) + C22_mat.similarity( mat_prod_2 );
756
757 C_cor_vec = Ajac_mat*DeltaA_vec + Bjac_mat*DeltaY_vec;
758 C_vec = C_cor_vec + F_vec;
759
760 DeltaY_vec = C31_mat.transpose()*C_vec;
761 DeltaA_vec = C32_mat.transpose()*C_vec;
762
763 for (
unsigned int i=0;
i<n_dim; ++
i)
764 {
765 ChiItr_vec(0,i) = DeltaY_vec(i);
766 }
767 ChiItr_mat = Wmeas0_mat.similarity( ChiItr_vec );
768 chi2New = ChiItr_mat(0,0);
769
770
771 frameOriginItr[0] += DeltaA_vec(0);
772 frameOriginItr[1] += DeltaA_vec(1);
773 frameOriginItr[2] += DeltaA_vec(2);
774 if (msgLvl(MSG::DEBUG)) {
775 msg(MSG::DEBUG) <<
"New vertex, global coordinates: " << frameOriginItr.transpose() <<
endmsg;
776 msg(MSG::DEBUG) <<
"chi2Old: " << chi2Old <<
" chi2New: " << chi2New <<
" fabs(chi2Old-chi2New): " << fabs(chi2Old-chi2New) <<
endmsg;
777 }
778
780 if (globalPositionItr->perp() >
m_maxR && globalPositionItr->z() >
m_maxZ)
return nullptr;
781
782 if (onConstr && fabs(chi2Old-chi2New) < 0.1) { break; }
783
784 double BFieldItr[3];
785 fieldCache.
getField(globalPositionItr->data(),BFieldItr);
786 double B_z_new = BFieldItr[2]*299.792;
787 if (B_z_new == 0. || std::isnan(B_z_new)) {
789 B_z_new = B_z;
790 }
791
792 restartFit = false;
793 double deltaR = sqrt(DeltaA_vec(0)*DeltaA_vec(0)+DeltaA_vec(1)*DeltaA_vec(1)+DeltaA_vec(2)*DeltaA_vec(2));
794 double deltaB_z = fabs(B_z-B_z_new)/B_z;
795 bool changeBz = false;
796
799 } else {
801 }
802
803 if (changeBz) {
804 B_z = B_z_new;
805
806 v0FitterTracks.clear();
807 Trk::PerigeeSurface perigeeSurfaceItr(*globalPositionItr);
808
810 {
811 if (chargeParameters != nullptr)
812 {
813
814 const Amg::Vector3D gMomentum = chargeParameters->momentum();
815 const Amg::Vector3D gDirection = chargeParameters->position() - *globalPositionItr;
816 const double extrapolationDirection = gMomentum .dot( gDirection );
819 std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(nullptr);
820
821 std::unique_ptr<const Trk::TrackParameters>
tmp =
822 std::abs(chargeParameters->position().z()) >
m_maxZ ? nullptr :
824 *chargeParameters,
825 perigeeSurfaceItr,
827 true,
829 mode);
830
831
833 extrapolatedPerigee.reset(
835 }
836
837 if (extrapolatedPerigee == nullptr) {
838 ATH_MSG_DEBUG(
"Perigee was not extrapolated! Taking original one!");
840 if (tmpPerigee!=nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
841 else return nullptr;
842 }
843
844
845 V0FitterTrack locV0FitterTrack;
846 locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[
Trk::d0];
847 locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[
Trk::z0];
848 locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[
Trk::phi];
849 locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[
Trk::theta];
850 locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[
Trk::qOverP];
851 locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
852 locV0FitterTrack.originalPerigee = chargeParameters;
853 v0FitterTracks.push_back(locV0FitterTrack);
854 } else {
855 ATH_MSG_DEBUG(
"Track parameters are not charged tracks ... fit aborted");
856 return nullptr;
857 }
858 }
859 frameOrigin = frameOriginItr;
860 Y0_vec *= 0.;
861 Y_vec *= 0.;
862 A_vec *= 0.;
863 DeltaY_vec *= 0.;
864 DeltaA_vec *= 0.;
865 chi2Old = 2000000000000.;
866 chi2New = 0.;
867 sumConstr = 0.;
868 onConstr = false;
869 restartFit = true;
870 }
871
872
873
874 }
875
876 frameOrigin[0] += DeltaA_vec(0);
877 frameOrigin[1] += DeltaA_vec(1);
878 frameOrigin[2] += DeltaA_vec(2);
879 if ( std::isnan(frameOrigin[0]) || std::isnan(frameOrigin[1]) || std::isnan(frameOrigin[2]) ) return nullptr;
880
881 Y_vec = Y0_vec + DeltaY_vec;
882
883
884 for (
unsigned int i=0;
i<nTrk; ++
i)
885 {
886 if ( fabs ( Y_vec(2+5*i) ) > 100. || fabs ( Y_vec(3+5*i) ) > 100. ) { return nullptr; }
887 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;
888 while ( Y_vec(3+5*i) > 2*
M_PI ) Y_vec(3+5*i) -= 2*
M_PI;
889 while ( Y_vec(3+5*i) < -
M_PI ) Y_vec(3+5*i) +=
M_PI;
890 if ( Y_vec(3+5*i) >
M_PI )
891 {
892 Y_vec(3+5*i) = 2*
M_PI - Y_vec(3+5*i);
893 if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -
M_PI :
M_PI;
894 }
895 if ( Y_vec(3+5*i) < 0.0 )
896 {
897 Y_vec(3+5*i) = - Y_vec(3+5*i);
898 if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -
M_PI :
M_PI;
899 }
900 }
901
902 for (
unsigned int i=0;
i<n_dim; ++
i)
903 {
904 Chi_vec(0,i) = DeltaY_vec(i);
905 }
906 Chi_mat = Wmeas0_mat.similarity( Chi_vec );
908
909 V_mat.setZero();
910 V_mat.block(0,0,n_dim,n_dim) = C11_mat;
911 V_mat.block<3,3>(n_dim,n_dim) = C22_mat;
912 V_mat.block(n_dim,0,3,n_dim) = C21_mat;
913 V_mat.block(0,n_dim,n_dim,3) = C21_mat.transpose();
914
915
916 std::vector<V0FitterTrack>::iterator BTIter;
917 int iRP=0;
918 for (BTIter = v0FitterTracks.begin(); BTIter != v0FitterTracks.end() ; ++BTIter)
919 {
920
921 AmgSymMatrix(5) covTrk = Wmeas0_mat.block<5,5>(5*iRP,5*iRP);
923 for (unsigned int i=0; i<5; ++i) chi_vec(i) = DeltaY_vec(i+5*iRP);
924 double chi2Trk = chi_vec.dot(covTrk*chi_vec);
925 (*BTIter).
chi2=chi2Trk;
926 iRP++;
927 }
928
929
930 auto vx = std::make_unique<xAOD::
Vertex>();
931 vx->makePrivateStore();
932 vx->setPosition (frameOrigin);
933 vx->setCovariancePosition (C22_mat);
934 vx->setFitQuality(
chi2,static_cast<
float>(ndf));
935 vx->setVertexType(xAOD::VxType::
V0Vtx);
936
937
938 std::vector<VxTrackAtVertex> & tracksAtVertex = vx->vxTrackAtVertex(); tracksAtVertex.
clear();
939 Amg::
Vector3D Vertex(frameOrigin[0],frameOrigin[1],frameOrigin[2]);
940 const Trk::PerigeeSurface Surface(
Vertex);
941 Trk::
Perigee * refittedPerigee(
nullptr);
942 unsigned int iterf=0;
943 std::vector<V0FitterTrack>::iterator BTIterf;
944 for (BTIterf = v0FitterTracks.begin(); BTIterf != v0FitterTracks.end() ; ++BTIterf)
945 {
946 AmgSymMatrix(5) CovMtxP = V_mat.block<5,5>(5*iterf, 5*iterf);
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),
948 Surface, std::move(CovMtxP));
949 tracksAtVertex.emplace_back((*BTIterf).
chi2, refittedPerigee, (*BTIterf).originalPerigee);
950 iterf++;
951 }
952
953
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);
960 }
961 }
962 vx->setCovariance(floatErrMtx);
963
964 return vx;
965 }
Scalar perp() const
perp method - perpendicular length
Scalar deltaR(const MatrixBase< Derived > &vec) const
#define ATH_MSG_ERROR(x,...)
#define ATH_MSG_VERBOSE(x,...)
double charge(const T &p)
#define AmgSymMatrix(dim)
void clear()
Empty the pool.
void getField(const double *ATH_RESTRICT xyz, double *ATH_RESTRICT bxyz, double *ATH_RESTRICT deriv=nullptr)
get B field value at given position xyz[3] is in mm, bxyz[3] is in kT if deriv[9] is given,...
SG::ReadCondHandleKey< AtlasFieldCacheCondObj > m_fieldCacheCondObjInputKey
double chi2(TH1 *h0, TH1 *h1)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorX
Dynamic Vector - dynamic allocation.
float j(const xAOD::IParticle &, const xAOD::TrackMeasurementValidation &hit, const Eigen::Matrix3d &jab_inv)
@ V0Vtx
Vertex from V0 Decay.
ParametersT< TrackParametersDim, Charged, PerigeeSurface > Perigee
@ z
global position (cartesian)
MaterialUpdateMode
This is a steering enum to force the material update it can be: (1) addNoise (-1) removeNoise Second ...
ParametersBase< TrackParametersDim, Charged > TrackParameters