276{
277 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
280
282 std::vector< Vect3DF > cVertices;
283 std::vector< std::vector<double> > covVertices;
284 std::vector< std::vector< VectMOM> > fittedParticles;
285 std::vector< std::vector<double> > fittedCovariance;
286 std::vector<double> fitFullCovariance;
287 std::vector<double> particleChi2;
288
289 int ntrk=0;
291 std::vector<const TrackParameters*> baseInpTrk;
293 std::vector<const xAOD::TrackParticle*>::const_iterator i_ntrk;
294
295 unsigned int indexFMP;
296 for (i_ntrk = cstate.m_partListForCascade.begin(); i_ntrk < cstate.m_partListForCascade.end(); ++i_ntrk) {
298 ATH_MSG_DEBUG(
"FirstMeasuredPoint on track is discovered. Use it.");
300 }else{
301 ATH_MSG_DEBUG(
"No FirstMeasuredPoint on track in CascadeFitter. Stop fit");
303 }
304 }
307 }else{
309 }
311
313
314 std::vector< std::vector<int> > vertexDefinition;
315 std::vector< std::vector<int> > cascadeDefinition;
317
318 double * partMass=new double[ntrk];
319 for(
int i=0;
i<ntrk;
i++) partMass[i] = cstate.m_partMassForCascade[i];
320 int IERR =
makeCascade(state.m_vkalFitControl, ntrk, state.m_ich, partMass, &state.m_apar[0][0], &state.m_awgt[0][0],
321 vertexDefinition,
322 cascadeDefinition,
324 CascadeEvent & refCascadeEvent=*(state.m_vkalFitControl.getCascadeEvent());
325
326
327
328 std::vector<int> indexT,indexV,indexTT,indexVV,tmpInd;
329 for (const PartialMassConstraint& c : cstate.m_partMassCnstForCascade) {
330
332 IERR =
findPositions(
c.trkInVrt, vertexDefinition[index], indexT);
if(IERR)
break;
333 tmpInd.clear();
334 for (
int idx :
c.pseudoInVrt)
336 IERR =
findPositions( tmpInd, cascadeDefinition[index], indexV);
if(IERR)
break;
337
339 if(IERR)break;
340 }
342 if(msgLvl(MSG::DEBUG)){
343 msg(MSG::DEBUG)<<
"Standard cascade fit" <<
endmsg;
345 }
346
347
348
349
350
351
352 if(primVrt){
353 double vertex[3] = {primVrt->position().x()-state.m_refFrameX, primVrt->position().y()-state.m_refFrameY,primVrt->position().z()-state.m_refFrameZ};
354 const RecVertex* primVrtRec=dynamic_cast< const RecVertex* > (primVrt);
355 if(primVrtRec){
356 double covari[6] = {primVrtRec->covariancePosition()(0,0),primVrtRec->covariancePosition()(0,1),
357 primVrtRec->covariancePosition()(1,1),primVrtRec->covariancePosition()(0,2),
358 primVrtRec->covariancePosition()(1,2),primVrtRec->covariancePosition()(2,2)};
361 }else{
363 }
364 }else{
366 }
368 getFittedCascade(refCascadeEvent, cVertices, covVertices, fittedParticles, fittedCovariance, particleChi2, fitFullCovariance );
369
370
371
372
373
374
375
376
377
378
379
380
381 int ip,ivFrom=0,ivTo;
383 for( ivTo=0; ivTo<(
int)vertexDefinition.size(); ivTo++){
384 if(cascadeDefinition[ivTo].
empty())
continue;
385 for( ip=0;
ip<(
int)cascadeDefinition[ivTo].size();
ip++){
386 ivFrom=cascadeDefinition[ivTo][
ip];
388 for(it=0;
it<(
int)fittedParticles[ivFrom].size();
it++){
389 px += fittedParticles[ivFrom][
it].Px;
390 py += fittedParticles[ivFrom][
it].Py;
391 pz += fittedParticles[ivFrom][
it].Pz;
392 }
393 Sign= (cVertices[ivFrom].X-cVertices[ivTo].X)*
px
394 +(cVertices[ivFrom].Y-cVertices[ivTo].Y)*
py
395 +(cVertices[ivFrom].Z-cVertices[ivTo].Z)*
pz;
397 }
399 }
400
401
402
403 int NDOFsqueezed=0;
406 std::vector< std::vector<int> > new_vertexDefinition;
407 std::vector< std::vector<int> > new_cascadeDefinition;
408 cstate.m_cascadeVList[ivFrom].mergedTO=cstate.m_cascadeVList[ivTo].vID;
409 cstate.m_cascadeVList[ivTo].mergedIN.push_back(ivFrom);
411 if(msgLvl(MSG::DEBUG)){
412 msg(MSG::DEBUG)<<
"Compressed cascade fit" <<
endmsg;
414 }
415
416 state.m_vkalFitControl.renewCascadeEvent(new CascadeEvent());
417 partMass=new double[ntrk];
418 for(
int i=0;
i<ntrk;
i++) partMass[i] = cstate.m_partMassForCascade[i];
419 int IERR =
makeCascade(state.m_vkalFitControl, ntrk, state.m_ich, partMass, &state.m_apar[0][0], &state.m_awgt[0][0],
420 new_vertexDefinition,
421 new_cascadeDefinition);
delete[] partMass;
if(IERR){
CLEANCASCADE();
return nullptr;}
422
423 for (const PartialMassConstraint& c : cstate.m_partMassCnstForCascade) {
424 indexT.clear(); indexV.clear();
426 IERR =
findPositions(
c.trkInVrt, new_vertexDefinition[index], indexT);
if(IERR)
break;
428 int icv=
indexInV(inV, cstate);
if(icv<0)
break;
429 if(cstate.m_cascadeVList[icv].mergedTO ==
c.VRT){
430 IERR =
findPositions(cstate.m_cascadeVList[icv].trkInVrt, new_vertexDefinition[index], indexTT);
431 if(IERR)break;
432 indexT.insert (indexT.end(), indexTT.begin(), indexTT.end());
433 }else{
434 std::vector<int> tmpI(1); tmpI[0]=inV;
435 IERR =
findPositions(tmpI, new_cascadeDefinition[index], indexVV);
436 if(IERR)break;
437 indexV.insert (indexV.end(), indexVV.begin(), indexVV.end());
438 }
439 } if(IERR)break;
440
441
442 IERR =
setCascadeMassConstraint(*(state.m_vkalFitControl.getCascadeEvent()), index , indexT, indexV,
c.Mass);
if(IERR)
break;
443 }
444 ATH_MSG_DEBUG(
"Setting compressed mass constraints ierr="<<IERR);
446
447
448
449 if(primVrt){
450 double vertex[3] = {primVrt->position().x()-state.m_refFrameX, primVrt->position().y()-state.m_refFrameY,primVrt->position().z()-state.m_refFrameZ};
451 const RecVertex* primVrtRec=dynamic_cast< const RecVertex* > (primVrt);
452 if(primVrtRec){
453 double covari[6] = {primVrtRec->covariancePosition()(0,0),primVrtRec->covariancePosition()(0,1),
454 primVrtRec->covariancePosition()(1,1),primVrtRec->covariancePosition()(0,2),
455 primVrtRec->covariancePosition()(1,2),primVrtRec->covariancePosition()(2,2)};
457 }else{
459 }
460 }else{
461 IERR =
processCascade(*(state.m_vkalFitControl.getCascadeEvent()));
462 }
465
466
467
468 std::vector< Vect3DF > t_cVertices;
469 std::vector< std::vector<double> > t_covVertices;
470 std::vector< std::vector< VectMOM> > t_fittedParticles;
471 std::vector< std::vector<double> > t_fittedCovariance;
472 std::vector<double> t_fitFullCovariance;
473 getFittedCascade(*(state.m_vkalFitControl.getCascadeEvent()), t_cVertices, t_covVertices, t_fittedParticles,
474 t_fittedCovariance, particleChi2, t_fitFullCovariance);
475 cVertices.clear(); covVertices.clear();
476
477
478
479 if(msgLvl(MSG::DEBUG)){
480 msg(MSG::DEBUG)<<
"Initial cascade momenta"<<
endmsg;
481 for(
int kv=0; kv<(
int)fittedParticles.size(); kv++){
482 for(
int kt=0;
kt<(
int)fittedParticles[kv].size();
kt++)
483 std::cout<<
484 " Px="<<fittedParticles[kv][kt].Px<<" Py="<<fittedParticles[kv][kt].Py<<";";
485 std::cout<<'\n';
486 }
487 msg(MSG::DEBUG)<<
"Squized cascade momenta"<<
endmsg;
488 for(
int kv=0; kv<(
int)t_fittedParticles.size(); kv++){
489 for(
int kt=0;
kt<(
int)t_fittedParticles[kv].size();
kt++)
490 std::cout<<
491 " Px="<<t_fittedParticles[kv][kt].Px<<" Py="<<t_fittedParticles[kv][kt].Py<<";";
492 std::cout<<'\n';
493 }
494 }
495 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
497 cVertices.push_back(t_cVertices[index]);
498 covVertices.push_back(t_covVertices[index]);
499 for(it=0;
it<(
int)cstate.m_cascadeVList[iv].trkInVrt.size();
it++){
500 int numTrk=cstate.m_cascadeVList[iv].trkInVrt[
it];
501 for(itmp=0; itmp<(
int)new_vertexDefinition[
index].size(); itmp++)
if(numTrk==new_vertexDefinition[index][itmp])
break;
502 fittedParticles[iv][
it]=t_fittedParticles[
index][itmp];
503
510 }
517 }
518
519
520 VectMOM tmpMom{};
521 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
523 int NTrkInVrt=cstate.m_cascadeVList[iv].trkInVrt.size();
524 for(ip=0;
ip<(
int)cstate.m_cascadeVList[iv].inPointingV.size();
ip++){
525 int tmpIndexV=
indexInV( cstate.m_cascadeVList[iv].inPointingV[ip], cstate);
526 if(cstate.m_cascadeVList[tmpIndexV].mergedTO){
527 tmpMom.Px=tmpMom.Py=tmpMom.Pz=tmpMom.E=0.;
528 for(it=0;
it<(
int)(cstate.m_cascadeVList[tmpIndexV].trkInVrt.size()+
529 cstate.m_cascadeVList[tmpIndexV].inPointingV.size()); it++){
530 tmpMom.Px += fittedParticles[tmpIndexV][
it].Px; tmpMom.Py += fittedParticles[tmpIndexV][
it].Py;
531 tmpMom.Pz += fittedParticles[tmpIndexV][
it].Pz; tmpMom.E += fittedParticles[tmpIndexV][
it].E;
532 }
533 fittedParticles[iv][
ip+NTrkInVrt]=tmpMom;
534 }else{
535 int indexS=
getSimpleVIndex( cstate.m_cascadeVList[iv].inPointingV[ip], cstate );
536 for(itmp=0; itmp<(
int)new_cascadeDefinition[
index].size(); itmp++)
if(indexS==new_cascadeDefinition[index][itmp])
break;
537 fittedParticles[iv][
ip+NTrkInVrt]=t_fittedParticles[
index][itmp+new_vertexDefinition[
index].size()];
538 }
539 }
540 }
541 if(msgLvl(MSG::DEBUG)){
542 msg(MSG::DEBUG)<<
"Refit cascade momenta"<<
endmsg;
543 for(
int kv=0; kv<(
int)fittedParticles.size(); kv++){
544 for(
int kt=0;
kt<(
int)fittedParticles[kv].size();
kt++)
545 std::cout<<
546 " Px="<<fittedParticles[kv][kt].Px<<" Py="<<fittedParticles[kv][kt].Py<<";";
547 std::cout<<'\n';
548 }
549 }
550
551
552
553 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
555 if(cstate.m_cascadeVList[iv].mergedTO)
isMerged=
true;
557 for(ip=0;
ip<(
int)cstate.m_cascadeVList[iv].inPointingV.size();
ip++){
558 int tmpIndexV=
indexInV( cstate.m_cascadeVList[iv].inPointingV[ip], cstate);
559 if(cstate.m_cascadeVList[tmpIndexV].mergedTO)
isMerged=
true;
560 }
561 if(!isMerged){
562 fittedCovariance[iv]=t_fittedCovariance[
index];
563 }
564 }
565 }
566
567
568
572 std::vector<xAOD::Vertex*> xaodVrtList(0);
574
575 int NDOF=
getCascadeNDoF(cstate);
if(NDOFsqueezed) NDOF=NDOFsqueezed;
576 if(primVrt){ if(FirstDecayAtPV){ NDOF+=3; }else{ NDOF+=2; } }
577
578 for(iv=0; iv<(
int)cVertices.size(); iv++){
579 Amg::Vector3D FitVertex(cVertices[iv].X+state.m_refFrameX,cVertices[iv].Y+state.m_refFrameY,cVertices[iv].Z+state.m_refFrameZ);
580 VrtCovMtx(0,0) = covVertices[iv][0]; VrtCovMtx(0,1) = covVertices[iv][1];
581 VrtCovMtx(1,1) = covVertices[iv][2]; VrtCovMtx(0,2) = covVertices[iv][3];
582 VrtCovMtx(1,2) = covVertices[iv][4]; VrtCovMtx(2,2) = covVertices[iv][5];
583 VrtCovMtx(1,0) = VrtCovMtx(0,1);
584 VrtCovMtx(2,0) = VrtCovMtx(0,2);
585 VrtCovMtx(2,1) = VrtCovMtx(1,2);
586 double Chi2=0;
587 for(it=0;
it<(
int)vertexDefinition[iv].size();
it++) { Chi2 += particleChi2[vertexDefinition[iv][
it]];};
588 fullChi2+=Chi2;
589
590
592 tmpXAODVertex->makePrivateStore();
595 std::vector<VxTrackAtVertex> & tmpVTAV=tmpXAODVertex->
vxTrackAtVertex();
596 tmpVTAV.clear();
597
598 int NRealT=vertexDefinition[iv].size();
600 for( it=0;
it<NRealT*3+3;
it++){
601 for( jt=0; jt<=
it; jt++){
602 genCOV(it,jt) = genCOV(jt,it) = fittedCovariance[iv][
it*(
it+1)/2+jt];
603 } }
606
607 fullDeriv=Amg::MatrixX::Zero(NRealT*3+3, NRealT*3+3);
608 fullDeriv(0,0)=fullDeriv(1,1)=fullDeriv(2,2)=1.;
609 }
610 for( it=0;
it<NRealT;
it++) {
611 mom= sqrt( fittedParticles[iv][it].Pz*fittedParticles[iv][it].Pz
612 +fittedParticles[iv][it].Py*fittedParticles[iv][it].Py
613 +fittedParticles[iv][it].Px*fittedParticles[iv][it].Px);
614 double Px=fittedParticles[iv][
it].Px;
615 double Py=fittedParticles[iv][
it].Py;
616 double Pz=fittedParticles[iv][
it].Pz;
617 double Pt= sqrt(Px*Px + Py*Py) ;
619 theta=acos( Pz/mom );
620 invP = - state.m_ich[vertexDefinition[iv][
it]] /
mom;
621
623 tmpDeriv.setZero();
624 tmpDeriv(0,1) = -
sin(
phi);
628 tmpDeriv(1,3) = 1.;
629 tmpDeriv(2+0,3*it+3+0) = -Py/
Pt/
Pt;
630 tmpDeriv(2+0,3*it+3+1) = Px/
Pt/
Pt;
631 tmpDeriv(2+0,3*it+3+2) = 0;
632 tmpDeriv(2+1,3*it+3+0) = Px*Pz/(
Pt*
mom*
mom);
633 tmpDeriv(2+1,3*it+3+1) = Py*Pz/(
Pt*
mom*
mom);
634 tmpDeriv(2+1,3*it+3+2) = -
Pt/(
mom*
mom);
635 tmpDeriv(2+2,3*it+3+0) = -Px/(
mom*
mom) * invP;
636 tmpDeriv(2+2,3*it+3+1) = -Py/(
mom*
mom) * invP;
637 tmpDeriv(2+2,3*it+3+2) = -Pz/(
mom*
mom) * invP;
638
640
643 measPerigee = new
Perigee( 0.,0.,
phi,
theta, invP, PerigeeSurface(FitVertex), std::move(tmpCovMtx) );
644 tmpVTAV.emplace_back( particleChi2[vertexDefinition[iv][it]] , measPerigee ) ;
645 }
646 std::vector<float> floatErrMtx;
649 tmpCovMtx=genCOV.similarity(fullDeriv);
650 floatErrMtx.resize((NRealT*3+3)*(NRealT*3+3+1)/2);
651 int ivk=0;
652 for(
int i=0;
i<NRealT*3+3;
i++){
653 for(
int j=0;
j<=
i;
j++){
654 floatErrMtx.at(ivk++)=tmpCovMtx(i,j);
655 }
656 }
657 }else{
658 floatErrMtx.resize(6);
659 for(
int i=0;
i<6;
i++) floatErrMtx[i]=covVertices[iv][i];
660 }
662 for(int itvk=0; itvk<NRealT; itvk++) {
663 ElementLink<xAOD::TrackParticleContainer> TEL;
664 if(itvk < (int)cstate.m_cascadeVList[iv].trkInVrt.size()){
665 TEL.
setElement( cstate.m_partListForCascade[ cstate.m_cascadeVList[iv].trkInVrt[itvk] ] );
666 }else{
668 }
670 }
671 xaodVrtList.push_back(tmpXAODVertex);
672
673 }
674
675
676
677 std::vector<TLorentzVector> tmpMoms;
678 std::vector<std::vector<TLorentzVector> > particleMoms;
679 std::vector<Amg::MatrixX> particleCovs;
680 int allFitPrt=0;
681 for(iv=0; iv<(
int)cVertices.size(); iv++){
682 tmpMoms.clear();
683 int NTrkF=fittedParticles[iv].size();
684 for(it=0;
it< NTrkF;
it++) {
685 tmpMoms.emplace_back( fittedParticles[iv][it].Px, fittedParticles[iv][it].Py,
686 fittedParticles[iv][it].Pz, fittedParticles[iv][it].E );
687 }
688
689 Amg::MatrixX COV(NTrkF*3+3,NTrkF*3+3); COV=Amg::MatrixX::Zero(NTrkF*3+3,NTrkF*3+3);
690 for( it=0;
it<NTrkF*3+3;
it++){
691 for( jt=0; jt<=
it; jt++){
692 COV(it,jt) = COV(jt,it) = fittedCovariance[iv][
it*(
it+1)/2+jt];
693 } }
694 particleMoms.push_back( std::move(tmpMoms) );
695 particleCovs.push_back( std::move(COV) );
696 allFitPrt += NTrkF;
697 }
698
699 int NAPAR=(allFitPrt+cVertices.size())*3;
700
702 if( !NDOFsqueezed ){
703 for( it=0;
it<NAPAR;
it++){
704 for( jt=0; jt<=
it; jt++){
705 FULL(it,jt) =
FULL(jt,it) = fitFullCovariance[
it*(
it+1)/2+jt];
706 } }
707 }else{
708
709 int mcount=0;
710 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
711
712 FULL.block(mcount,mcount,particleCovs[iv].
rows(),particleCovs[iv].
cols())=particleCovs[iv];
713 mcount += particleCovs[iv].rows();
714 }
715 }
716
717
718
719 VxCascadeInfo * recCascade= new VxCascadeInfo(std::move(xaodVrtList),std::move(particleMoms),std::move(particleCovs), NDOF ,fullChi2);
720 recCascade->setFullCascadeCovariance(FULL);
722 return recCascade;
723}
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.16.1