280{
281 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
284
286 std::vector< Vect3DF > cVertices;
287 std::vector< std::vector<double> > covVertices;
288 std::vector< std::vector< VectMOM> > fittedParticles;
289 std::vector< std::vector<double> > fittedCovariance;
290 std::vector<double> fitFullCovariance;
291 std::vector<double> particleChi2;
292
293 int ntrk=0;
295 std::vector<const TrackParameters*> baseInpTrk;
297 std::vector<const xAOD::TrackParticle*>::const_iterator i_ntrk;
298
299 unsigned int indexFMP;
300 for (i_ntrk = cstate.m_partListForCascade.begin(); i_ntrk < cstate.m_partListForCascade.end(); ++i_ntrk) {
302 ATH_MSG_DEBUG(
"FirstMeasuredPoint on track is discovered. Use it.");
304 }else{
305 ATH_MSG_DEBUG(
"No FirstMeasuredPoint on track in CascadeFitter. Stop fit");
307 }
308 }
311 }else{
313 }
315
317
318 std::vector< std::vector<int> > vertexDefinition;
319 std::vector< std::vector<int> > cascadeDefinition;
321
322 double * partMass=new double[ntrk];
323 for(
int i=0;
i<ntrk;
i++) partMass[i] = cstate.m_partMassForCascade[i];
324 int IERR =
makeCascade(state.m_vkalFitControl, ntrk, state.m_ich, partMass, &state.m_apar[0][0], &state.m_awgt[0][0],
325 vertexDefinition,
326 cascadeDefinition,
328 CascadeEvent & refCascadeEvent=*(state.m_vkalFitControl.getCascadeEvent());
329
330
331
332 std::vector<int> indexT,indexV,indexTT,indexVV,tmpInd;
333 for (const PartialMassConstraint& c : cstate.m_partMassCnstForCascade) {
334
337 throw std::runtime_error("TrkVKalVrtFitter::fitCascade: index into vector is negative.");
338 }
339 IERR =
findPositions(
c.trkInVrt, vertexDefinition[index], indexT);
340 if(IERR)break;
341 tmpInd.clear();
342 for (
int idx :
c.pseudoInVrt)
344 IERR =
findPositions( tmpInd, cascadeDefinition[index], indexV);
if(IERR)
break;
345
347 if(IERR)break;
348 }
350 if(msgLvl(MSG::DEBUG)){
351 msg(MSG::DEBUG)<<
"Standard cascade fit" <<
endmsg;
353 }
354
355
356
357
358
359
360 if(primVrt){
361 double vertex[3] = {primVrt->position().x()-state.m_refFrameX, primVrt->position().y()-state.m_refFrameY,primVrt->position().z()-state.m_refFrameZ};
362 const RecVertex* primVrtRec=dynamic_cast< const RecVertex* > (primVrt);
363 if(primVrtRec){
364 double covari[6] = {primVrtRec->covariancePosition()(0,0),primVrtRec->covariancePosition()(0,1),
365 primVrtRec->covariancePosition()(1,1),primVrtRec->covariancePosition()(0,2),
366 primVrtRec->covariancePosition()(1,2),primVrtRec->covariancePosition()(2,2)};
369 }else{
371 }
372 }else{
374 }
376 getFittedCascade(refCascadeEvent, cVertices, covVertices, fittedParticles, fittedCovariance, particleChi2, fitFullCovariance );
377
378
379
380
381
382
383
384
385
386
387
388
389 int ip,ivFrom=0,ivTo;
391 for( ivTo=0; ivTo<(
int)vertexDefinition.size(); ivTo++){
392 if(cascadeDefinition[ivTo].
empty())
continue;
393 for( ip=0;
ip<(
int)cascadeDefinition[ivTo].size();
ip++){
394 ivFrom=cascadeDefinition[ivTo][
ip];
396 for(it=0;
it<(
int)fittedParticles[ivFrom].size();
it++){
397 px += fittedParticles[ivFrom][
it].Px;
398 py += fittedParticles[ivFrom][
it].Py;
399 pz += fittedParticles[ivFrom][
it].Pz;
400 }
401 Sign= (cVertices[ivFrom].X-cVertices[ivTo].X)*
px
402 +(cVertices[ivFrom].Y-cVertices[ivTo].Y)*
py
403 +(cVertices[ivFrom].Z-cVertices[ivTo].Z)*
pz;
405 }
407 }
408
409
410
411 int NDOFsqueezed=0;
414 std::vector< std::vector<int> > new_vertexDefinition;
415 std::vector< std::vector<int> > new_cascadeDefinition;
416 cstate.m_cascadeVList[ivFrom].mergedTO=cstate.m_cascadeVList[ivTo].vID;
417 cstate.m_cascadeVList[ivTo].mergedIN.push_back(ivFrom);
419 if(msgLvl(MSG::DEBUG)){
420 msg(MSG::DEBUG)<<
"Compressed cascade fit" <<
endmsg;
422 }
423
424 state.m_vkalFitControl.renewCascadeEvent(new CascadeEvent());
425 partMass=new double[ntrk];
426 for(
int i=0;
i<ntrk;
i++) partMass[i] = cstate.m_partMassForCascade[i];
427 int IERR =
makeCascade(state.m_vkalFitControl, ntrk, state.m_ich, partMass, &state.m_apar[0][0], &state.m_awgt[0][0],
428 new_vertexDefinition,
429 new_cascadeDefinition);
delete[] partMass;
if(IERR){
CLEANCASCADE();
return nullptr;}
430
431 for (const PartialMassConstraint& c : cstate.m_partMassCnstForCascade) {
432 indexT.clear(); indexV.clear();
434 IERR =
findPositions(
c.trkInVrt, new_vertexDefinition[index], indexT);
if(IERR)
break;
436 int icv=
indexInV(inV, cstate);
if(icv<0)
break;
437 if(cstate.m_cascadeVList[icv].mergedTO ==
c.VRT){
438 IERR =
findPositions(cstate.m_cascadeVList[icv].trkInVrt, new_vertexDefinition[index], indexTT);
439 if(IERR)break;
440 indexT.insert (indexT.end(), indexTT.begin(), indexTT.end());
441 }else{
442 std::vector<int> tmpI(1); tmpI[0]=inV;
443 IERR =
findPositions(tmpI, new_cascadeDefinition[index], indexVV);
444 if(IERR)break;
445 indexV.insert (indexV.end(), indexVV.begin(), indexVV.end());
446 }
447 } if(IERR)break;
448
449
450 IERR =
setCascadeMassConstraint(*(state.m_vkalFitControl.getCascadeEvent()), index , indexT, indexV,
c.Mass);
if(IERR)
break;
451 }
452 ATH_MSG_DEBUG(
"Setting compressed mass constraints ierr="<<IERR);
454
455
456
457 if(primVrt){
458 double vertex[3] = {primVrt->position().x()-state.m_refFrameX, primVrt->position().y()-state.m_refFrameY,primVrt->position().z()-state.m_refFrameZ};
459 const RecVertex* primVrtRec=dynamic_cast< const RecVertex* > (primVrt);
460 if(primVrtRec){
461 double covari[6] = {primVrtRec->covariancePosition()(0,0),primVrtRec->covariancePosition()(0,1),
462 primVrtRec->covariancePosition()(1,1),primVrtRec->covariancePosition()(0,2),
463 primVrtRec->covariancePosition()(1,2),primVrtRec->covariancePosition()(2,2)};
465 }else{
467 }
468 }else{
469 IERR =
processCascade(*(state.m_vkalFitControl.getCascadeEvent()));
470 }
473
474
475
476 std::vector< Vect3DF > t_cVertices;
477 std::vector< std::vector<double> > t_covVertices;
478 std::vector< std::vector< VectMOM> > t_fittedParticles;
479 std::vector< std::vector<double> > t_fittedCovariance;
480 std::vector<double> t_fitFullCovariance;
481 getFittedCascade(*(state.m_vkalFitControl.getCascadeEvent()), t_cVertices, t_covVertices, t_fittedParticles,
482 t_fittedCovariance, particleChi2, t_fitFullCovariance);
483 cVertices.clear(); covVertices.clear();
484
485
486
487 if(msgLvl(MSG::DEBUG)){
488 msg(MSG::DEBUG)<<
"Initial cascade momenta"<<
endmsg;
489 for(
int kv=0; kv<(
int)fittedParticles.size(); kv++){
490 for(
int kt=0;
kt<(
int)fittedParticles[kv].size();
kt++)
491 std::cout<<
492 " Px="<<fittedParticles[kv][kt].Px<<" Py="<<fittedParticles[kv][kt].Py<<";";
493 std::cout<<'\n';
494 }
495 msg(MSG::DEBUG)<<
"Squized cascade momenta"<<
endmsg;
496 for(
int kv=0; kv<(
int)t_fittedParticles.size(); kv++){
497 for(
int kt=0;
kt<(
int)t_fittedParticles[kv].size();
kt++)
498 std::cout<<
499 " Px="<<t_fittedParticles[kv][kt].Px<<" Py="<<t_fittedParticles[kv][kt].Py<<";";
500 std::cout<<'\n';
501 }
502 }
503 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
505 cVertices.push_back(t_cVertices[index]);
506 covVertices.push_back(t_covVertices[index]);
507 for(it=0;
it<(
int)cstate.m_cascadeVList[iv].trkInVrt.size();
it++){
508 int numTrk=cstate.m_cascadeVList[iv].trkInVrt[
it];
509 for(itmp=0; itmp<(
int)new_vertexDefinition[
index].size(); itmp++)
if(numTrk==new_vertexDefinition[index][itmp])
break;
510 fittedParticles[iv][
it]=t_fittedParticles[
index][itmp];
511
518 }
525 }
526
527
528 VectMOM tmpMom{};
529 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
531 int NTrkInVrt=cstate.m_cascadeVList[iv].trkInVrt.size();
532 for(ip=0;
ip<(
int)cstate.m_cascadeVList[iv].inPointingV.size();
ip++){
533 int tmpIndexV=
indexInV( cstate.m_cascadeVList[iv].inPointingV[ip], cstate);
534 if(cstate.m_cascadeVList[tmpIndexV].mergedTO){
535 tmpMom.Px=tmpMom.Py=tmpMom.Pz=tmpMom.E=0.;
536 for(it=0;
it<(
int)(cstate.m_cascadeVList[tmpIndexV].trkInVrt.size()+
537 cstate.m_cascadeVList[tmpIndexV].inPointingV.size()); it++){
538 tmpMom.Px += fittedParticles[tmpIndexV][
it].Px; tmpMom.Py += fittedParticles[tmpIndexV][
it].Py;
539 tmpMom.Pz += fittedParticles[tmpIndexV][
it].Pz; tmpMom.E += fittedParticles[tmpIndexV][
it].E;
540 }
541 fittedParticles[iv][
ip+NTrkInVrt]=tmpMom;
542 }else{
543 int indexS=
getSimpleVIndex( cstate.m_cascadeVList[iv].inPointingV[ip], cstate );
544 for(itmp=0; itmp<(
int)new_cascadeDefinition[
index].size(); itmp++)
if(indexS==new_cascadeDefinition[index][itmp])
break;
545 fittedParticles[iv][
ip+NTrkInVrt]=t_fittedParticles[
index][itmp+new_vertexDefinition[
index].size()];
546 }
547 }
548 }
549 if(msgLvl(MSG::DEBUG)){
550 msg(MSG::DEBUG)<<
"Refit cascade momenta"<<
endmsg;
551 for(
int kv=0; kv<(
int)fittedParticles.size(); kv++){
552 for(
int kt=0;
kt<(
int)fittedParticles[kv].size();
kt++)
553 std::cout<<
554 " Px="<<fittedParticles[kv][kt].Px<<" Py="<<fittedParticles[kv][kt].Py<<";";
555 std::cout<<'\n';
556 }
557 }
558
559
560
561 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
563 if(cstate.m_cascadeVList[iv].mergedTO)
isMerged=
true;
565 for(ip=0;
ip<(
int)cstate.m_cascadeVList[iv].inPointingV.size();
ip++){
566 int tmpIndexV=
indexInV( cstate.m_cascadeVList[iv].inPointingV[ip], cstate);
567 if(cstate.m_cascadeVList[tmpIndexV].mergedTO)
isMerged=
true;
568 }
569 if(!isMerged){
570 fittedCovariance[iv]=t_fittedCovariance[
index];
571 }
572 }
573 }
574
575
576
580 std::vector<xAOD::Vertex*> xaodVrtList(0);
582
583 int NDOF=
getCascadeNDoF(cstate);
if(NDOFsqueezed) NDOF=NDOFsqueezed;
584 if(primVrt){ if(FirstDecayAtPV){ NDOF+=3; }else{ NDOF+=2; } }
585
586 for(iv=0; iv<(
int)cVertices.size(); iv++){
587 Amg::Vector3D FitVertex(cVertices[iv].X+state.m_refFrameX,cVertices[iv].Y+state.m_refFrameY,cVertices[iv].Z+state.m_refFrameZ);
588 VrtCovMtx(0,0) = covVertices[iv][0]; VrtCovMtx(0,1) = covVertices[iv][1];
589 VrtCovMtx(1,1) = covVertices[iv][2]; VrtCovMtx(0,2) = covVertices[iv][3];
590 VrtCovMtx(1,2) = covVertices[iv][4]; VrtCovMtx(2,2) = covVertices[iv][5];
591 VrtCovMtx(1,0) = VrtCovMtx(0,1);
592 VrtCovMtx(2,0) = VrtCovMtx(0,2);
593 VrtCovMtx(2,1) = VrtCovMtx(1,2);
594 double Chi2=0;
595 for(it=0;
it<(
int)vertexDefinition[iv].size();
it++) { Chi2 += particleChi2[vertexDefinition[iv][
it]];};
596 fullChi2+=Chi2;
597
598
600 tmpXAODVertex->makePrivateStore();
603 std::vector<VxTrackAtVertex> & tmpVTAV=tmpXAODVertex->
vxTrackAtVertex();
604 tmpVTAV.clear();
605
606 int NRealT=vertexDefinition[iv].size();
608 for( it=0;
it<NRealT*3+3;
it++){
609 for( jt=0; jt<=
it; jt++){
610 genCOV(it,jt) = genCOV(jt,it) = fittedCovariance[iv][
it*(
it+1)/2+jt];
611 } }
614
615 fullDeriv=Amg::MatrixX::Zero(NRealT*3+3, NRealT*3+3);
616 fullDeriv(0,0)=fullDeriv(1,1)=fullDeriv(2,2)=1.;
617 }
618 for( it=0;
it<NRealT;
it++) {
619 mom= sqrt( fittedParticles[iv][it].Pz*fittedParticles[iv][it].Pz
620 +fittedParticles[iv][it].Py*fittedParticles[iv][it].Py
621 +fittedParticles[iv][it].Px*fittedParticles[iv][it].Px);
622 double Px=fittedParticles[iv][
it].Px;
623 double Py=fittedParticles[iv][
it].Py;
624 double Pz=fittedParticles[iv][
it].Pz;
625 double Pt= sqrt(Px*Px + Py*Py) ;
627 theta=acos( Pz/mom );
628 invP = - state.m_ich[vertexDefinition[iv][
it]] /
mom;
629
631 tmpDeriv.setZero();
632 tmpDeriv(0,1) = -
sin(
phi);
636 tmpDeriv(1,3) = 1.;
637 tmpDeriv(2+0,3*it+3+0) = -Py/
Pt/
Pt;
638 tmpDeriv(2+0,3*it+3+1) = Px/
Pt/
Pt;
639 tmpDeriv(2+0,3*it+3+2) = 0;
640 tmpDeriv(2+1,3*it+3+0) = Px*Pz/(
Pt*
mom*
mom);
641 tmpDeriv(2+1,3*it+3+1) = Py*Pz/(
Pt*
mom*
mom);
642 tmpDeriv(2+1,3*it+3+2) = -
Pt/(
mom*
mom);
643 tmpDeriv(2+2,3*it+3+0) = -Px/(
mom*
mom) * invP;
644 tmpDeriv(2+2,3*it+3+1) = -Py/(
mom*
mom) * invP;
645 tmpDeriv(2+2,3*it+3+2) = -Pz/(
mom*
mom) * invP;
646
648
651 measPerigee = new
Perigee( 0.,0.,
phi,
theta, invP, PerigeeSurface(FitVertex), std::move(tmpCovMtx) );
652 tmpVTAV.emplace_back( particleChi2[vertexDefinition[iv][it]] , measPerigee ) ;
653 }
654 std::vector<float> floatErrMtx;
657 tmpCovMtx=genCOV.similarity(fullDeriv);
658 floatErrMtx.resize((NRealT*3+3)*(NRealT*3+3+1)/2);
659 int ivk=0;
660 for(
int i=0;
i<NRealT*3+3;
i++){
661 for(
int j=0;
j<=
i;
j++){
662 floatErrMtx.at(ivk++)=tmpCovMtx(i,j);
663 }
664 }
665 }else{
666 floatErrMtx.resize(6);
667 for(
int i=0;
i<6;
i++) floatErrMtx[i]=covVertices[iv][i];
668 }
670 for(int itvk=0; itvk<NRealT; itvk++) {
671 ElementLink<xAOD::TrackParticleContainer> TEL;
672 if(itvk < (int)cstate.m_cascadeVList[iv].trkInVrt.size()){
673 TEL.
setElement( cstate.m_partListForCascade[ cstate.m_cascadeVList[iv].trkInVrt[itvk] ] );
674 }else{
676 }
678 }
679 xaodVrtList.push_back(tmpXAODVertex);
680
681 }
682
683
684
685 std::vector<TLorentzVector> tmpMoms;
686 std::vector<std::vector<TLorentzVector> > particleMoms;
687 std::vector<Amg::MatrixX> particleCovs;
688 int allFitPrt=0;
689 for(iv=0; iv<(
int)cVertices.size(); iv++){
690 tmpMoms.clear();
691 int NTrkF=fittedParticles[iv].size();
692 for(it=0;
it< NTrkF;
it++) {
693 tmpMoms.emplace_back( fittedParticles[iv][it].Px, fittedParticles[iv][it].Py,
694 fittedParticles[iv][it].Pz, fittedParticles[iv][it].E );
695 }
696
697 Amg::MatrixX COV(NTrkF*3+3,NTrkF*3+3); COV=Amg::MatrixX::Zero(NTrkF*3+3,NTrkF*3+3);
698 for( it=0;
it<NTrkF*3+3;
it++){
699 for( jt=0; jt<=
it; jt++){
700 COV(it,jt) = COV(jt,it) = fittedCovariance[iv][
it*(
it+1)/2+jt];
701 } }
702 particleMoms.push_back( std::move(tmpMoms) );
703 particleCovs.push_back( std::move(COV) );
704 allFitPrt += NTrkF;
705 }
706
707 int NAPAR=(allFitPrt+cVertices.size())*3;
708
710 if( !NDOFsqueezed ){
711 for( it=0;
it<NAPAR;
it++){
712 for( jt=0; jt<=
it; jt++){
713 FULL(it,jt) =
FULL(jt,it) = fitFullCovariance[
it*(
it+1)/2+jt];
714 } }
715 }else{
716
717 int mcount=0;
718 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
719
720 FULL.block(mcount,mcount,particleCovs[iv].
rows(),particleCovs[iv].
cols())=particleCovs[iv];
721 mcount += particleCovs[iv].rows();
722 }
723 }
724
725
726
727 VxCascadeInfo * recCascade= new VxCascadeInfo(std::move(xaodVrtList),std::move(particleMoms),std::move(particleCovs), NDOF ,fullChi2);
728 recCascade->setFullCascadeCovariance(FULL);
730 return recCascade;
731}
Matrix< Scalar, OtherDerived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime > similarity(const MatrixBase< OtherDerived > &m) const
similarity method : yields ms = m*s*m^T
static const Attributes_t empty
static int getSimpleVIndex(const VertexID &, const CascadeState &cstate)
StatusCode CvtTrackParameters(const std::vector< const TrackParameters * > &InpTrk, int &ntrk, State &state) const
void VKalVrtConfigureFitterCore(int NTRK, State &state) const
Gaudi::Property< bool > m_makeExtendedVertex
static int findPositions(const std::vector< int > &, const std::vector< int > &, std::vector< int > &)
static void makeSimpleCascade(std::vector< std::vector< int > > &, std::vector< std::vector< int > > &, CascadeState &cstate)
static void printSimpleCascade(std::vector< std::vector< int > > &, std::vector< std::vector< int > > &, const CascadeState &cstate)
Gaudi::Property< double > m_cascadeCnstPrecision
StatusCode CvtTrackParticle(std::span< const xAOD::TrackParticle *const > list, int &ntrk, State &state) const
static int getCascadeNDoF(const CascadeState &cstate)
void addTrackAtVertex(const ElementLink< TrackParticleContainer > &tr, float weight=1.0)
Add a new track to the vertex.
void setCovariance(const std::vector< float > &value)
Sets the covariance matrix as a simple vector of values.
void setPosition(const Amg::Vector3D &position)
Sets the 3-position.
std::vector< Trk::VxTrackAtVertex > & vxTrackAtVertex()
Non-const access to the VxTrackAtVertex vector.
void setFitQuality(float chiSquared, float numberDoF)
Set the 'Fit Quality' information.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
bool isMerged(int matchInfo)
int SymIndex(int it, int i, int j)
void getFittedCascade(CascadeEvent &cascadeEvent_, std::vector< Vect3DF > &cVertices, std::vector< std::vector< double > > &covVertices, std::vector< std::vector< VectMOM > > &fittedParticles, std::vector< std::vector< double > > &cascadeCovar, std::vector< double > &particleChi2, std::vector< double > &fullCovar)
int processCascade(CascadeEvent &cascadeEvent_)
int setCascadeMassConstraint(CascadeEvent &cascadeEvent_, long int IV, double Mass)
CurvilinearParametersT< TrackParametersDim, Charged, PlaneSurface > CurvilinearParameters
int makeCascade(VKalVrtControl &FitCONTROL, long int NTRK, const long int *ich, double *wm, double *inp_Trk5, double *inp_CovTrk5, const std::vector< std::vector< int > > &vertexDefinition, const std::vector< std::vector< int > > &cascadeDefinition, double definedCnstAccuracy)
@ pz
global momentum (cartesian)
int processCascadePV(CascadeEvent &cascadeEvent_, const double *primVrt, const double *primVrtCov)
Vertex_v1 Vertex
Define the latest version of the vertex class.
@ FirstMeasurement
Parameter defined at the position of the 1st measurement.
1.17.0