ATLAS Offline Software
VKalVrtFitSvc.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2023 CERN for the benefit of the ATLAS collaboration
3 */
4 
5 // Header include
8 #include "TrkVKalVrtCore/CFit.h"
10 #include "TrkVKalVrtCore/cfPEst.h"
11 //-------------------------------------------------
12 // Other stuff
13 #include "GaudiKernel/IChronoStatSvc.h"
14 //
15 #include <algorithm>
16 #include <cmath>
17 
18  namespace Trk {
19 
20 //__________________________________________________________________________
21 //&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
22 // Interface
23 //
24 
25  StatusCode TrkVKalVrtFitter::VKalVrtFit(const std::vector<const Perigee*>& InpPerigee,
27  TLorentzVector& Momentum,
28  long int& Charge,
29  dvect& ErrorMatrix,
30  dvect& Chi2PerTrk,
31  std::vector< std::vector<double> >& TrkAtVrt,
32  double& Chi2,
33  IVKalState& istate,
34  bool ifCovV0 /*= false*/) const
35 {
36  assert(dynamic_cast<State*> (&istate)!=nullptr);
37  State& state = static_cast<State&> (istate);
38 //
39 //------ extract information about selected tracks
40 //
41 
42  int ntrk=0;
43  StatusCode sc = CvtPerigee(InpPerigee, ntrk, state);
44  if(sc.isFailure())return StatusCode::FAILURE;
45 
46  int ierr = VKalVrtFit3( ntrk, Vertex, Momentum, Charge, ErrorMatrix,
47  Chi2PerTrk, TrkAtVrt,Chi2, state, ifCovV0 ) ;
48  if (ierr) return StatusCode::FAILURE;
49  return StatusCode::SUCCESS;
50 }
51 
52 
53 
54 StatusCode TrkVKalVrtFitter::VKalVrtFit(const std::vector<const xAOD::TrackParticle*> & InpTrkC,
55  const std::vector<const xAOD::NeutralParticle*> & InpTrkN,
57  TLorentzVector& Momentum,
58  long int& Charge,
59  dvect& ErrorMatrix,
60  dvect& Chi2PerTrk,
61  std::vector< std::vector<double> >& TrkAtVrt,
62  double& Chi2,
63  IVKalState& istate,
64  bool ifCovV0 /*= false*/) const
65 {
66  assert(dynamic_cast<State*> (&istate)!=nullptr);
67  State& state = static_cast<State&> (istate);
68 
69 //
70 //------ extract information about selected tracks
71 //
72  int ntrk=0;
73 
74  // The tmpInputC will be not owning just holding plain ptr
75  // the ownership is handled via the TParamOwner
76  // and is unique_ptr so we do not leak
77  std::vector<const TrackParameters*> tmpInputC(0);
78  std::vector<std::unique_ptr<const TrackParameters>> TParamOwner(0);
79  StatusCode sc;
80  double closestHitR=1.e6; //VK needed for FirstMeasuredPointLimit if this hit itself is absent
81  if(m_firstMeasuredPoint){ //First measured point strategy
82  //------
83  if(!InpTrkC.empty()){
84  if( m_InDetExtrapolator == nullptr ){
85  if(msgLvl(MSG::WARNING))msg()<< "No InDet extrapolator given."<<
86  "Can't use FirstMeasuredPoint with xAOD::TrackParticle!!!" << endmsg;
87  return StatusCode::FAILURE;
88  }
89  std::vector<const xAOD::TrackParticle*>::const_iterator i_ntrk;
90  if(msgLvl(MSG::DEBUG))msg()<< "Start FirstMeasuredPoint handling"<<'\n';
91  unsigned int indexFMP;
92  for (i_ntrk = InpTrkC.begin(); i_ntrk < InpTrkC.end(); ++i_ntrk) {
93  if ((*i_ntrk)->indexOfParameterAtPosition(indexFMP, xAOD::FirstMeasurement)){
94  if(msgLvl(MSG::DEBUG))msg()<< "FirstMeasuredPoint on track is discovered. Use it."<<'\n';
95  // create parameters
96  TParamOwner.emplace_back(std::make_unique<CurvilinearParameters>(
97  (*i_ntrk)->curvilinearParameters(indexFMP)));
98  //For the last one we created, push also a not owning / view ptr to tmpInputC
99  tmpInputC.push_back((TParamOwner.back()).get());
100  }else{
101  if(msgLvl(MSG::DEBUG)){
102  msg()<< "FirstMeasuredPoint on track is absent."<<
103  "Try extrapolation from Perigee to FisrtMeasuredPoint radius"<<endmsg;
104  }
105 
106  TParamOwner.emplace_back(m_fitPropagator->myxAODFstPntOnTrk((*i_ntrk)));
107  //For the last one we created, push also a not owning / view ptr to tmpInputC
108  tmpInputC.push_back((TParamOwner.back()).get());
109  if(tmpInputC[tmpInputC.size()-1]==nullptr){
110  //Extrapolation failure
111  if(msgLvl(MSG::WARNING)){
112  msg()<< "InDetExtrapolator can't etrapolate xAOD::TrackParticle Perigee "<<
113  "to FirstMeasuredPoint radius! Stop vertex fit!" << endmsg;
114  }
115  return StatusCode::FAILURE;
116  }
117  }
118  if( (*i_ntrk)->radiusOfFirstHit() < closestHitR ) {
119  closestHitR=(*i_ntrk)->radiusOfFirstHit();
120  }
121  }
122  sc=CvtTrackParameters(tmpInputC,ntrk,state);
123  if(sc.isFailure()){
124  return StatusCode::FAILURE;
125  }
126  }
127  }else{
128  if(!InpTrkC.empty()) {
129  sc=CvtTrackParticle(InpTrkC,ntrk,state);
130  }
131  }
132  if(sc.isFailure())return StatusCode::FAILURE;
133  if(!InpTrkN.empty()){sc=CvtNeutralParticle(InpTrkN,ntrk,state); if(sc.isFailure())return StatusCode::FAILURE;}
134  //--
135  int ierr = VKalVrtFit3( ntrk, Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt, Chi2, state, ifCovV0 ) ;
136  if (ierr) return StatusCode::FAILURE;
137  //
138  //-- Check vertex position with respect to first measured hit and refit with plane constraint if needed
139  state.m_planeCnstNDOF = 0;
141  Amg::Vector3D cnstRefPoint(0.,0.,0.);
142  if(closestHitR==1.e6){ // Not found previously
143  const xAOD::TrackParticle * trkFMP=nullptr;
144  for(auto &trka : InpTrkC){
145  double hitR=trka->radiusOfFirstHit();
146  if(closestHitR>hitR){
147  closestHitR=hitR;
148  trkFMP=trka;
149  }
150  }
151  if(closestHitR<1.e6){
152  auto perFMP=m_fitPropagator->myxAODFstPntOnTrk(trkFMP); //FMP is calculated by extrapolation to radiusOfFirstHit
153  if(perFMP) cnstRefPoint=perFMP->position();
154  }
155  }
156  Amg::Vector3D unitMom=Amg::Vector3D(Momentum.Px()/Momentum.P(),Momentum.Py()/Momentum.P(),Momentum.Pz()/Momentum.P());
157  //----------- Use as reference either hit(state.m_globalFirstHit) or its radius(closestHitR) if hit is absent
158  if(state.m_globalFirstHit)cnstRefPoint=state.m_globalFirstHit->position();
159  //------------
160  if(Vertex.perp()>closestHitR && cnstRefPoint.perp()>0.){
161  if(msgLvl(MSG::DEBUG))msg(MSG::DEBUG)<<"Vertex behind first measured point is detected. Constraint is applied!"<<endmsg;
162  state.m_planeCnstNDOF = 1; // Additional NDOF due to plane constraint
163  double D= unitMom.x()*(cnstRefPoint.x()-state.m_refFrameX)
164  +unitMom.y()*(cnstRefPoint.y()-state.m_refFrameY)
165  +unitMom.z()*(cnstRefPoint.z()-state.m_refFrameZ);
166  state.m_parPlaneCnst[0]=unitMom.x();
167  state.m_parPlaneCnst[1]=unitMom.y();
168  state.m_parPlaneCnst[2]=unitMom.z();
169  state.m_parPlaneCnst[3]=D;
170  state.m_cnstRadius=std::sqrt(std::pow(cnstRefPoint.x()-state.m_refFrameX,2.)+std::pow(cnstRefPoint.y()-state.m_refFrameY,2.));
171  state.m_cnstRadiusRef[0]=-state.m_refFrameX;
172  state.m_cnstRadiusRef[1]=-state.m_refFrameY;
173  std::vector<double> saveApproxV(3,0.); state.m_ApproximateVertex.swap(saveApproxV);
174  state.m_ApproximateVertex[0]=cnstRefPoint.x();
175  state.m_ApproximateVertex[1]=cnstRefPoint.y();
176  state.m_ApproximateVertex[2]=cnstRefPoint.z();
177  ierr = VKalVrtFit3( ntrk, Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt, Chi2, state, ifCovV0 );
178  state.m_vkalFitControl.setUsePlaneCnst(0.,0.,0.,0.);
179  if (ierr) { // refit without plane cnst
180  ierr = VKalVrtFit3(ntrk,Vertex,Momentum,Charge,ErrorMatrix,Chi2PerTrk,TrkAtVrt,Chi2, state, ifCovV0); // if fit with it failed
181  state.m_planeCnstNDOF = 0;
182  }
183  state.m_ApproximateVertex.swap(saveApproxV);
184  }
185  }
186  //--
187  if (ierr) return StatusCode::FAILURE;
188  return StatusCode::SUCCESS;
189 }
190 
191 
192 StatusCode TrkVKalVrtFitter::VKalVrtFit(const std::vector<const TrackParameters*> & InpTrkC,
193  const std::vector<const NeutralParameters*> & InpTrkN,
195  TLorentzVector& Momentum,
196  long int& Charge,
197  dvect& ErrorMatrix,
198  dvect& Chi2PerTrk,
199  std::vector< std::vector<double> >& TrkAtVrt,
200  double& Chi2,
201  IVKalState& istate,
202  bool ifCovV0 /*= false*/) const
203 {
204  assert(dynamic_cast<State*> (&istate)!=nullptr);
205  State& state = static_cast<State&> (istate);
206 
207 //
208 //------ extract information about selected tracks
209 //
210  int ntrk=0;
211  StatusCode sc;
212  if(!InpTrkC.empty()){
213  sc=CvtTrackParameters(InpTrkC,ntrk,state);
214  if(sc.isFailure())return StatusCode::FAILURE;
215  }
216  if(!InpTrkN.empty()){
217  sc=CvtNeutralParameters(InpTrkN,ntrk,state);
218  if(sc.isFailure())return StatusCode::FAILURE;
219  }
220 
221  if(state.m_ApproximateVertex.empty() && state.m_globalFirstHit){ //Initial guess if absent
222  state.m_ApproximateVertex.reserve(3);
223  state.m_ApproximateVertex.push_back(state.m_globalFirstHit->position().x());
224  state.m_ApproximateVertex.push_back(state.m_globalFirstHit->position().y());
225  state.m_ApproximateVertex.push_back(state.m_globalFirstHit->position().z());
226  }
227  int ierr = VKalVrtFit3( ntrk, Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt,Chi2, state, ifCovV0 ) ;
228  if (ierr) return StatusCode::FAILURE;
229 //
230 //-- Check vertex position with respect to first measured hit and refit with plane constraint if needed
231  state.m_planeCnstNDOF = 0;
232  if(state.m_globalFirstHit && m_firstMeasuredPointLimit && !ierr){
233  if(Vertex.perp()>state.m_globalFirstHit->position().perp()){
234  if(msgLvl(MSG::DEBUG))msg(MSG::DEBUG)<<"Vertex behind first measured point is detected. Constraint is applied!"<<endmsg;
235  state.m_planeCnstNDOF = 1; // Additional NDOF due to plane constraint
236  double pp[3]={Momentum.Px()/Momentum.Rho(),Momentum.Py()/Momentum.Rho(),Momentum.Pz()/Momentum.Rho()};
237  double D= pp[0]*(state.m_globalFirstHit->position().x()-state.m_refFrameX)
238  +pp[1]*(state.m_globalFirstHit->position().y()-state.m_refFrameY)
239  +pp[2]*(state.m_globalFirstHit->position().z()-state.m_refFrameZ);
240  state.m_vkalFitControl.setUsePlaneCnst( pp[0], pp[1], pp[2], D);
241  std::vector<double> saveApproxV(3,0.); state.m_ApproximateVertex.swap(saveApproxV);
242  state.m_ApproximateVertex[0]=state.m_globalFirstHit->position().x();
243  state.m_ApproximateVertex[1]=state.m_globalFirstHit->position().y();
244  state.m_ApproximateVertex[2]=state.m_globalFirstHit->position().z();
245  ierr = VKalVrtFit3( ntrk, Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt,Chi2, state, ifCovV0 ) ;
246  state.m_vkalFitControl.setUsePlaneCnst(0.,0.,0.,0.);
247  if (ierr) { // refit without plane cnst
248  ierr = VKalVrtFit3(ntrk,Vertex,Momentum,Charge,ErrorMatrix,Chi2PerTrk,TrkAtVrt,Chi2, state, ifCovV0 ) ; // if fit with it failed
249  state.m_planeCnstNDOF = 0;
250  }
251  state.m_ApproximateVertex.swap(saveApproxV);
252  }
253  }
254  if (ierr) return StatusCode::FAILURE;
255  return StatusCode::SUCCESS;
256 }
257 
258 
259 
260 
261 
262 
263 //--------------------------------------------------------------------------------------------------
264 // Main code
265 //
268  TLorentzVector& Momentum,
269  long int& Charge,
271  dvect& Chi2PerTrk,
272  std::vector< std::vector<double> >& TrkAtVrt,
273  double& Chi2,
274  State& state,
275  bool ifCovV0) const
276 {
277 //
278 //------ Variables and arrays needed for fitting kernel
279 //
280  int ierr,i;
281  double xyz0[3],covf[21],chi2f=-10.;
282  double ptot[4]={0.};
283  double xyzfit[3]={0.};
284 //
285 //--- Set field value at (0.,0.,0.) - some safety
286 //
287  double Bx,By,Bz;
288  state.m_fitField.getMagFld(-state.m_refFrameX,-state.m_refFrameY,-state.m_refFrameZ,Bx,By,Bz);
289 //
290 //------ Fit option setting
291 //
292  VKalVrtConfigureFitterCore(ntrk, state);
293 //
294 //------ Fit itself
295 //
296  state.m_FitStatus=0;
297  state.m_vkalFitControl.renewFullCovariance(nullptr); //
298  state.m_vkalFitControl.setVertexMass(-1.);
300  if(state.m_ApproximateVertex.size()==3 && fabs(state.m_ApproximateVertex[2])<m_IDsizeZ &&
302  {
303  xyz0[0]=(double)state.m_ApproximateVertex[0] - state.m_refFrameX;
304  xyz0[1]=(double)state.m_ApproximateVertex[1] - state.m_refFrameY;
305  xyz0[2]=(double)state.m_ApproximateVertex[2] - state.m_refFrameZ;
306  } else {
307  xyz0[0]=xyz0[1]=xyz0[2]=0.;
308  }
309  double par0[NTrMaxVFit][3]; //used only for fit preparation
310  Trk::cfpest( ntrk, xyz0, state.m_ich, state.m_apar, par0);
311 
312  Chi2PerTrk.resize (ntrk);
313  ierr=Trk::CFit( &state.m_vkalFitControl, ifCovV0, ntrk, state.m_ich, xyz0, par0, state.m_apar, state.m_awgt,
314  xyzfit, state.m_parfs, ptot, covf, chi2f,
315  Chi2PerTrk.data());
316 
317  if(msgLvl(MSG::DEBUG))msg(MSG::DEBUG) << "VKalVrt fit status="<<ierr<<" Chi2="<<chi2f<<endmsg;
318 
319  Chi2 = 100000000.;
320  if(ierr){
321  return ierr;
322  }
323  if(ptot[0]*ptot[0]+ptot[1]*ptot[1] == 0.) return -5; // Bad (divergent) fit
324 //
325 // Postfit operation. Creation of array for different error calculations and full error matrix copy
326 //
327  state.m_FitStatus=ntrk;
328  if(ifCovV0 && state.m_vkalFitControl.getFullCovariance()){ //If full fit error matrix is returned by VKalVrtCORE
329  int SymCovMtxSize=(3*ntrk+3)*(3*ntrk+4)/2;
330  state.m_ErrMtx.assign (state.m_vkalFitControl.getFullCovariance(),
331  state.m_vkalFitControl.getFullCovariance()+SymCovMtxSize);
332  state.m_vkalFitControl.renewFullCovariance(nullptr);
333  ErrorMatrix.clear(); ErrorMatrix.reserve(21); ErrorMatrix.assign(covf,covf+21);
334  } else {
335  ErrorMatrix.clear(); ErrorMatrix.reserve(6); ErrorMatrix.assign(covf,covf+6);
336  }
337 //---------------------------------------------------------------------------
338  Momentum.SetPxPyPzE( ptot[0], ptot[1], ptot[2], ptot[3] );
339  Chi2 = (double) chi2f;
340 
341  Vertex[0]= xyzfit[0] + state.m_refFrameX;
342  Vertex[1]= xyzfit[1] + state.m_refFrameY;
343  Vertex[2]= xyzfit[2] + state.m_refFrameZ;
344 
345  double sizeR = state.m_allowUltraDisplaced ? m_MSsizeR : m_IDsizeR;
346  double sizeZ = state.m_allowUltraDisplaced ? m_MSsizeZ : m_IDsizeZ;
347  if (Vertex.perp() > sizeR || std::abs(Vertex.z()) > sizeZ) return -5; // Solution outside acceptable volume due to divergence
348 
349  state.m_save_xyzfit[0]=xyzfit[0]; // saving of vertex position
350  state.m_save_xyzfit[1]=xyzfit[1]; // for full error matrix
351  state.m_save_xyzfit[2]=xyzfit[2];
352 //
353 // ------ Magnetic field in fitted vertex
354 //
355  double fx,fy,BMAG_CUR;
356  state.m_fitField.getMagFld(xyzfit[0] ,xyzfit[1] ,xyzfit[2] ,fx,fy,BMAG_CUR);
357  if(fabs(BMAG_CUR) < 0.01) BMAG_CUR=0.01; // Safety
358 
359  Charge=0; for(i=0; i<ntrk; i++){Charge+=state.m_ich[i];};
360  Charge=-Charge; //VK 30.11.2009 Change sign acoording to ATLAS
361 
362 
363  TrkAtVrt.clear(); TrkAtVrt.reserve(ntrk);
364  for(i=0; i<ntrk; i++){
365  std::vector<double> TrkPar(3);
366  VKalToTrkTrack(BMAG_CUR,(double)state.m_parfs[i][0],(double)state.m_parfs[i][1],(double) state.m_parfs[i][2],
367  TrkPar[0],TrkPar[1],TrkPar[2]);
368  TrkPar[2] = -TrkPar[2]; // Change of sign needed
369  TrkAtVrt.push_back( TrkPar );
370  }
371  return 0;
372  }
373 
374 
375 
376 // Converts Vertex, Mom, CovVrtMom in GLOBAL SYSTEM into perigee
377 //
378 //
379 
381  const TLorentzVector& Momentum,
382  const dvect& CovVrtMom,
383  const long int& Charge,
384  dvect& Perigee,
385  dvect& CovPerigee,
386  IVKalState& istate) const
387  {
388  assert(dynamic_cast<State*> (&istate)!=nullptr);
389  State& state = static_cast<State&> (istate);
390  int i,j,ij;
391  double Vrt[3],PMom[4],Cov0[21],Per[5],CovPer[15];
392 
393  for(i=0; i<3; i++) Vrt[i]=Vertex[i];
394  for(i=0; i<3; i++) PMom[i]=Momentum[i];
395  for(ij=i=0; i<6; i++){
396  for(j=0; j<=i; j++){
397  Cov0[ij]=CovVrtMom[ij];
398  ij++;
399  }
400  }
401  state.m_refFrameX=state.m_refFrameY=state.m_refFrameZ=0.; //VK Work in ATLAS ref frame ONLY!!!
402  long int vkCharge=-Charge; //VK 30.11.2009 Change sign according to ATLAS
403 //
404 // ------ Magnetic field in vertex
405 //
406  double fx,fy,BMAG_CUR;
407  state.m_fitField.getMagFld(Vrt[0], Vrt[1], Vrt[2] ,fx,fy,BMAG_CUR);
408  if(fabs(BMAG_CUR) < 0.01) BMAG_CUR=0.01; // Safety
409 
410  Trk::xyztrp( vkCharge, Vrt, PMom, Cov0, BMAG_CUR, Per, CovPer );
411 
412  Perigee.clear();
413  CovPerigee.clear();
414 
415  for(i=0; i<5; i++) Perigee.push_back((double)Per[i]);
416  for(i=0; i<15; i++) CovPerigee.push_back((double)CovPer[i]);
417 
418  return StatusCode::SUCCESS;
419  }
420 
421 
422  void TrkVKalVrtFitter::VKalToTrkTrack( double curBMAG, double vp1, double vp2, double vp3,
423  double& tp1, double& tp2, double& tp3) const
424 //tp - ATLAS parameters, vp - VKalVrt parameters//
425  { tp1= vp2; //phi angle
426  tp2= vp1; //theta angle
427  tp3= vp3 * std::sin( vp1 ) /(m_CNVMAG*curBMAG);
428  constexpr double pi = M_PI;
429  // -pi < phi < pi range
430  while ( tp1 > pi) tp1 -= 2.*pi;
431  while ( tp1 <-pi) tp1 += 2.*pi;
432  // 0 < Theta < pi range
433  while ( tp2 > pi) tp2 -= 2.*pi;
434  while ( tp2 <-pi) tp2 += 2.*pi;
435  if ( tp2 < 0.) {
436  tp2 = fabs(tp2); tp1 += pi;
437  while ( tp1 > pi) tp1 -= 2.*pi;
438  }
439 
440  }
441 
442 
443 /* Returns a complete error matrix after fit
444  useMom=0 def (V,Perigee1,Perigee2....PerigeeNTrk)
445  useMom=1 (V,PxPyPz1,PxPyPz2....PxPyPzNTrk)
446 */
447 
448 
449  StatusCode
450  TrkVKalVrtFitter::VKalGetFullCov( long int NTrk, dvect& CovVrtTrk,
451  IVKalState& istate,
452  bool useMom) const
453  {
454  assert(dynamic_cast<State*> (&istate)!=nullptr);
455  State& state = static_cast<State&> (istate);
456  if(!state.m_FitStatus) return StatusCode::FAILURE;
457  if(NTrk<1) return StatusCode::FAILURE;
458  if(NTrk>NTrMaxVFit) return StatusCode::FAILURE;
459  if(state.m_ErrMtx.empty()) return StatusCode::FAILURE; //Now error matrix is taken from CORE in VKalVrtFit3.
460 //
461 // ------ Magnetic field access
462 //
463  double fx,fy,BMAG_CUR;
464  state.m_fitField.getMagFld(state.m_save_xyzfit[0],state.m_save_xyzfit[1],state.m_save_xyzfit[2],fx,fy,BMAG_CUR);
465  if(fabs(BMAG_CUR) < 0.01) BMAG_CUR=0.01; // Safety
466 //
467 // ------ Base code
468 //
469  int i,j,ik,jk,ip,iTrk;
470  int DIM=3*NTrk+3; //Current size of full covariance matrix
471  std::vector<std::vector<double> > Deriv (DIM);
472  for (std::vector<double>& v : Deriv) v.resize (DIM);
473  std::vector<double> CovMtxOld(DIM*DIM);
474 
475 
476  CovVrtTrk.resize(DIM*(DIM+1)/2);
477 
478  ip=0;
479  for( i=0; i<DIM;i++) {
480  for( j=0; j<=i; j++) {
481  CovMtxOld[i*DIM+j]=CovMtxOld[j*DIM+i]=state.m_ErrMtx[ip++];
482  }
483  }
484 
485  //delete [] ErrMtx;
486 
487  for(i=0;i<DIM;i++){ for(j=0;j<DIM;j++) {Deriv[i][j]=0.;}}
488  Deriv[0][0]= 1.;
489  Deriv[1][1]= 1.;
490  Deriv[2][2]= 1.;
491 
492  int iSt=0;
493  double Theta,invR,Phi;
494  for( iTrk=0; iTrk<NTrk; iTrk++){
495  Theta=state.m_parfs[iTrk][0];
496  Phi =state.m_parfs[iTrk][1];
497  invR =state.m_parfs[iTrk][2];
498  /*-----------*/
499  /* dNew/dOld */
500  iSt = 3 + iTrk*3;
501  if( !useMom ){
502  Deriv[iSt ][iSt+1] = 1; // Phi <-> Theta
503  Deriv[iSt+1][iSt ] = 1; // Phi <-> Theta
504  Deriv[iSt+2][iSt ] = -(cos(Theta)/(m_CNVMAG*BMAG_CUR)) * invR ; // d1/p / dTheta
505  Deriv[iSt+2][iSt+2] = -(sin(Theta)/(m_CNVMAG*BMAG_CUR)) ; // d1/p / d1/R
506  }else{
507  double pt=(m_CNVMAG*BMAG_CUR)/fabs(invR);
508  double px=pt*cos(Phi);
509  double py=pt*sin(Phi);
510  double pz=pt/tan(Theta);
511  Deriv[iSt ][iSt ]= 0; //dPx/dTheta
512  Deriv[iSt ][iSt+1]= -py; //dPx/dPhi
513  Deriv[iSt ][iSt+2]= -px/invR; //dPx/dinvR
514 
515  Deriv[iSt+1][iSt ]= 0; //dPy/dTheta
516  Deriv[iSt+1][iSt+1]= px; //dPy/dPhi
517  Deriv[iSt+1][iSt+2]= -py/invR; //dPy/dinvR
518 
519  Deriv[iSt+2][iSt ]= -pt/sin(Theta)/sin(Theta); //dPz/dTheta
520  Deriv[iSt+2][iSt+1]= 0; //dPz/dPhi
521  Deriv[iSt+2][iSt+2]= -pz/invR; //dPz/dinvR
522  }
523  }
524 //---------- Only upper half if filled and saved
525  int ipnt=0;
526  double tmp, tmpTmp;
527  for(i=0;i<DIM;i++){
528  for(j=0;j<=i;j++){
529  tmp=0.;
530  for(ik=0;ik<DIM;ik++){
531  if(Deriv[i][ik] == 0.) continue;
532  tmpTmp=0;
533  for(jk=DIM-1;jk>=0;jk--){
534  if(Deriv[j][jk] == 0.) continue;
535  tmpTmp += CovMtxOld[ik*DIM+jk]*Deriv[j][jk];
536  }
537  tmp += Deriv[i][ik]*tmpTmp;
538  }
539  CovVrtTrk[ipnt++]=tmp;
540  }}
541 
542  return StatusCode::SUCCESS;
543 
544  }
545 
546 
547 
548 
549 
550  StatusCode TrkVKalVrtFitter::VKalGetMassError( double& dM, double& MassError,
551  const IVKalState& istate) const
552  {
553  assert(dynamic_cast<const State*> (&istate)!=nullptr);
554  const State& state = static_cast<const State&> (istate);
555  if(!state.m_FitStatus) return StatusCode::FAILURE;
556  dM = state.m_vkalFitControl.getVertexMass();
557  MassError = state.m_vkalFitControl.getVrtMassError();
558  return StatusCode::SUCCESS;
559  }
560 
561 
563  const IVKalState& istate) const
564  {
565  assert(dynamic_cast<const State*> (&istate)!=nullptr);
566  const State& state = static_cast<const State&> (istate);
567  if(!state.m_FitStatus) return StatusCode::FAILURE; // no fit made
568  trkWeights.clear();
569 
570  int NTRK=state.m_FitStatus;
571 
572  for (int i=0; i<NTRK; i++) trkWeights.push_back(state.m_vkalFitControl.vk_forcft.robres[i]);
573 
574  return StatusCode::SUCCESS;
575  }
576 
577 
579  {
580  if(!state.m_FitStatus) return 0;
581  int NDOF=2*state.m_FitStatus-3;
582  if(state.m_usePointingCnst) { NDOF+=2; }
583  else if(state.m_useZPointingCnst) { NDOF+=1; }
584  if( state.m_usePassNear || state.m_usePassWithTrkErr ) { NDOF+= 2; }
585 
586  if( state.m_massForConstraint>0. ) { NDOF+=1; }
587  if( !state.m_partMassCnst.empty() ) { NDOF+= state.m_partMassCnst.size(); }
588  if( state.m_useAprioriVertex ) { NDOF+= 3; }
589  if( state.m_usePhiCnst ) { NDOF+=1; }
590  if( state.m_useThetaCnst ) { NDOF+=1; }
591  return NDOF;
592  }
593 
594 }
Trk::TrkVKalVrtFitter::State
Definition: TrkVKalVrtFitter.h:387
Trk::TrkVKalVrtFitter::VKalVrtFit
virtual StatusCode VKalVrtFit(const std::vector< const xAOD::TrackParticle * > &, const std::vector< const xAOD::NeutralParticle * > &, Amg::Vector3D &Vertex, TLorentzVector &Momentum, long int &Charge, dvect &ErrorMatrix, dvect &Chi2PerTrk, std::vector< std::vector< double >> &TrkAtVrt, double &Chi2, IVKalState &istate, bool ifCovV0=false) const override final
Trk::Vertex
Definition: Tracking/TrkEvent/VxVertex/VxVertex/Vertex.h:26
Trk::py
@ py
Definition: ParamDefs.h:60
Trk::TrkVKalVrtFitter::CvtNeutralParameters
StatusCode CvtNeutralParameters(const std::vector< const NeutralParameters * > &InpTrk, int &ntrk, State &state) const
Definition: CvtParametersBase.cxx:179
Trk::TrkVKalVrtFitter::CvtNeutralParticle
StatusCode CvtNeutralParticle(const std::vector< const xAOD::NeutralParticle * > &list, int &ntrk, State &state) const
Definition: CvtTrackParticle.cxx:124
Trk::TrkVKalVrtFitter::State::m_useZPointingCnst
bool m_useZPointingCnst
Definition: TrkVKalVrtFitter.h:431
Trk::VKContraintType::Theta
@ Theta
Trk::NTrMaxVFit
@ NTrMaxVFit
Definition: TrkVKalVrtFitter.h:35
Trk::TrkVKalVrtFitter::State::m_refFrameZ
double m_refFrameZ
Definition: TrkVKalVrtFitter.h:395
xAOD::Vertex
Vertex_v1 Vertex
Define the latest version of the vertex class.
Definition: Event/xAOD/xAODTracking/xAODTracking/Vertex.h:16
Trk::TrkVKalVrtFitter::State::m_refFrameX
double m_refFrameX
Definition: TrkVKalVrtFitter.h:393
Trk::TrkVKalVrtFitter::m_InDetExtrapolator
const IExtrapolator * m_InDetExtrapolator
Pointer to Extrapolator AlgTool.
Definition: TrkVKalVrtFitter.h:480
Trk::ParametersT
Dummy class used to allow special convertors to be called for surfaces owned by a detector element.
Definition: EMErrorDetail.h:25
Trk::TrkVKalVrtFitter::CvtTrackParameters
StatusCode CvtTrackParameters(const std::vector< const TrackParameters * > &InpTrk, int &ntrk, State &state) const
Definition: CvtParametersBase.cxx:24
test_pyathena.pt
pt
Definition: test_pyathena.py:11
Trk::VKalVrtControl::setVrtMassError
void setVrtMassError(double error)
Definition: TrkVKalVrtCore.h:77
Trk::VKalVrtControl::getVrtMassError
double getVrtMassError() const
Definition: TrkVKalVrtCore.h:79
M_PI
#define M_PI
Definition: ActiveFraction.h:11
Trk::TrkVKalVrtFitter::m_fitPropagator
VKalExtPropagator * m_fitPropagator
Definition: TrkVKalVrtFitter.h:479
Trk::TrkVKalVrtFitter::m_IDsizeR
Gaudi::Property< double > m_IDsizeR
Definition: TrkVKalVrtFitter.h:326
CFit.h
Trk::VKalAtlasMagFld::getMagFld
virtual void getMagFld(const double, const double, const double, double &, double &, double &) override
Definition: VKalAtlasMagFld.cxx:61
Trk::TrkVKalVrtFitter::CvtPerigee
StatusCode CvtPerigee(const std::vector< const Perigee * > &list, int &ntrk, State &state) const
Definition: CvtPerigee.cxx:25
xAOD::Vertex_v1::position
const Amg::Vector3D & position() const
Returns the 3-pos.
Phi
@ Phi
Definition: RPCdef.h:8
Trk::TrkVKalVrtFitter::m_MSsizeZ
Gaudi::Property< double > m_MSsizeZ
Definition: TrkVKalVrtFitter.h:329
drawFromPickle.cos
cos
Definition: drawFromPickle.py:36
Trk::pz
@ pz
global momentum (cartesian)
Definition: ParamDefs.h:61
Trk::TrkVKalVrtFitter::State::m_FitStatus
int m_FitStatus
Definition: TrkVKalVrtFitter.h:415
Trk::TrkVKalVrtFitter::VKalGetNDOF
static int VKalGetNDOF(const State &state)
Definition: VKalVrtFitSvc.cxx:578
Trk::TrkVKalVrtFitter::State::m_save_xyzfit
double m_save_xyzfit[3]
Definition: TrkVKalVrtFitter.h:413
Trk::TrkVKalVrtFitter::VKalGetFullCov
virtual StatusCode VKalGetFullCov(long int, dvect &CovMtx, IVKalState &istate, bool=false) const override final
Definition: VKalVrtFitSvc.cxx:450
Trk::TrkVKalVrtFitter::State::m_partMassCnst
std::vector< double > m_partMassCnst
Definition: TrkVKalVrtFitter.h:440
Trk::TrkVKalVrtFitter::m_MSsizeR
Gaudi::Property< double > m_MSsizeR
Definition: TrkVKalVrtFitter.h:328
Trk::TrkVKalVrtFitter::State::m_vkalFitControl
VKalVrtControl m_vkalFitControl
Definition: TrkVKalVrtFitter.h:407
Trk::dvect
std::vector< double > dvect
Definition: TrkVKalVrtFitter.h:36
pi
#define pi
Definition: TileMuonFitter.cxx:65
TrkVKalVrtFitter.h
AthenaPoolTestRead.sc
sc
Definition: AthenaPoolTestRead.py:27
Trk::ErrorMatrix
Definition: ErrorMatrixCnv_p1.h:25
Trk::TrkVKalVrtFitter::VKalGetMassError
virtual StatusCode VKalGetMassError(double &Mass, double &MassError, const IVKalState &istate) const override final
Definition: VKalVrtFitSvc.cxx:550
Trk::VKalExtPropagator::myxAODFstPntOnTrk
const Perigee * myxAODFstPntOnTrk(const xAOD::TrackParticle *xprt) const
Definition: VKalExtPropagator.cxx:437
Trk::VKalVrtControl::setVertexMass
void setVertexMass(double mass)
Definition: TrkVKalVrtCore.h:76
Trk::TrkVKalVrtFitter::m_CNVMAG
double m_CNVMAG
Definition: TrkVKalVrtFitter.h:476
Trk::TrkVKalVrtFitter::State::m_useThetaCnst
bool m_useThetaCnst
Definition: TrkVKalVrtFitter.h:428
Trk::xyztrp
void xyztrp(const long int ich, double *vrt0, double *pv0, double *covi, double BMAG, double *paro, double *errt)
Definition: XYZtrp.cxx:16
lumiFormat.i
int i
Definition: lumiFormat.py:85
xAOD::FirstMeasurement
@ FirstMeasurement
Parameter defined at the position of the 1st measurement.
Definition: TrackingPrimitives.h:214
endmsg
#define endmsg
Definition: AnalysisConfig_Ntuple.cxx:63
EL::StatusCode
::StatusCode StatusCode
StatusCode definition for legacy code.
Definition: PhysicsAnalysis/D3PDTools/EventLoop/EventLoop/StatusCode.h:22
xAOD::TrackParticle_v1::radiusOfFirstHit
float radiusOfFirstHit() const
Returns the radius of the first hit.
MuonR4::State
CalibratedSpacePoint::State State
Definition: SpacePointCalibrator.cxx:33
Trk::TrkVKalVrtFitter::State::m_awgt
double m_awgt[NTrMaxVFit][15]
Definition: TrkVKalVrtFitter.h:402
TrkVKalVrtCore.h
Trk::px
@ px
Definition: ParamDefs.h:59
find_tgc_unfilled_channelids.ip
ip
Definition: find_tgc_unfilled_channelids.py:3
Trk::TrkVKalVrtFitter::State::m_allowUltraDisplaced
bool m_allowUltraDisplaced
Definition: TrkVKalVrtFitter.h:435
Trk::TrkVKalVrtFitter::State::m_useAprioriVertex
bool m_useAprioriVertex
Definition: TrkVKalVrtFitter.h:427
drawFromPickle.tan
tan
Definition: drawFromPickle.py:36
xAOD::double
double
Definition: CompositeParticle_v1.cxx:159
DeMoUpdate.tmp
string tmp
Definition: DeMoUpdate.py:1167
Trk::TrkVKalVrtFitter::m_firstMeasuredRadiusLimit
Gaudi::Property< bool > m_firstMeasuredRadiusLimit
Definition: TrkVKalVrtFitter.h:343
Trk::VKalVrtControl::renewFullCovariance
void renewFullCovariance(double *)
Definition: TrkVKalVrtCoreBase.cxx:320
Trk::cfpest
void cfpest(int ntrk, double *xyz, long int *ich, double(*parst)[5], double(*parf)[3])
Definition: cfPEst.cxx:10
dvect
std::vector< double > dvect
Definition: InDetVKalVxInJetTool.h:83
Trk::TrkVKalVrtFitter::State::m_usePhiCnst
bool m_usePhiCnst
Definition: TrkVKalVrtFitter.h:429
Trk
Ensure that the ATLAS eigen extensions are properly loaded.
Definition: FakeTrackBuilder.h:9
Trk::TrkVKalVrtFitter::VKalToTrkTrack
void VKalToTrkTrack(double curBMAG, double vp1, double vp2, double vp3, double &tp1, double &tp2, double &tp3) const
Definition: VKalVrtFitSvc.cxx:422
Trk::VKalVrtControl::getFullCovariance
const double * getFullCovariance() const
Definition: TrkVKalVrtCore.h:74
Trk::TrkVKalVrtFitter::State::m_fitField
VKalAtlasMagFld m_fitField
Definition: TrkVKalVrtFitter.h:406
Trk::TrkVKalVrtFitter::State::m_usePassNear
bool m_usePassNear
Definition: TrkVKalVrtFitter.h:432
Trk::TrkVKalVrtFitter::State::m_ich
long int m_ich[NTrMaxVFit]
Definition: TrkVKalVrtFitter.h:403
Amg::Vector3D
Eigen::Matrix< double, 3, 1 > Vector3D
Definition: GeoPrimitives.h:47
Trk::TrkVKalVrtFitter::VKalGetTrkWeights
virtual StatusCode VKalGetTrkWeights(dvect &Weights, const IVKalState &istate) const override final
Definition: VKalVrtFitSvc.cxx:562
Trk::TrkVKalVrtFitter::VKalVrtFit3
int VKalVrtFit3(int ntrk, Amg::Vector3D &Vertex, TLorentzVector &Momentum, long int &Charge, dvect &ErrorMatrix, dvect &Chi2PerTrk, std::vector< std::vector< double >> &TrkAtVrt, double &Chi2, State &state, bool ifCovV0) const
Definition: VKalVrtFitSvc.cxx:266
Trk::TrkVKalVrtFitter::State::m_refFrameY
double m_refFrameY
Definition: TrkVKalVrtFitter.h:394
Trk::TrkVKalVrtFitter::State::m_usePassWithTrkErr
bool m_usePassWithTrkErr
Definition: TrkVKalVrtFitter.h:433
Trk::TrkVKalVrtFitter::m_firstMeasuredPointLimit
Gaudi::Property< bool > m_firstMeasuredPointLimit
Definition: TrkVKalVrtFitter.h:342
Trk::IVKalState
Definition: IVKalState.h:21
Trk::TrkVKalVrtFitter::State::m_parfs
double m_parfs[NTrMaxVFit][3]
Definition: TrkVKalVrtFitter.h:404
Trk::TrkVKalVrtFitter::State::m_ApproximateVertex
std::vector< double > m_ApproximateVertex
Definition: TrkVKalVrtFitter.h:425
Trk::VKalVrtControl::getVertexMass
double getVertexMass() const
Definition: TrkVKalVrtCore.h:78
DEBUG
#define DEBUG
Definition: page_access.h:11
Trk::TrkVKalVrtFitter::VKalVrtConfigureFitterCore
void VKalVrtConfigureFitterCore(int NTRK, State &state) const
Definition: SetFitOptions.cxx:18
XYZtrp.h
Trk::CFit
int CFit(VKalVrtControl *FitCONTROL, int ifCovV0, int NTRK, long int *ich, double xyz0[3], double(*par0)[3], double(*inp_Trk5)[5], double(*inp_CovTrk5)[15], double xyzfit[3], double(*parfs)[3], double ptot[4], double covf[21], double &chi2, double *chi2tr)
Definition: CFit.cxx:435
Trk::TrkVKalVrtFitter::State::m_massForConstraint
double m_massForConstraint
Definition: TrkVKalVrtFitter.h:439
xAOD::TrackParticle_v1
Class describing a TrackParticle.
Definition: TrackParticle_v1.h:43
Trk::TrkVKalVrtFitter::VKalVrtCvtTool
virtual StatusCode VKalVrtCvtTool(const Amg::Vector3D &Vertex, const TLorentzVector &Momentum, const dvect &CovVrtMom, const long int &Charge, dvect &Perigee, dvect &CovPerigee, IVKalState &istate) const override final
Definition: VKalVrtFitSvc.cxx:380
drawFromPickle.sin
sin
Definition: drawFromPickle.py:36
Trk::TrkVKalVrtFitter::CvtTrackParticle
StatusCode CvtTrackParticle(std::span< const xAOD::TrackParticle *const > list, int &ntrk, State &state) const
Definition: CvtTrackParticle.cxx:27
Trk::TrkVKalVrtFitter::m_firstMeasuredPoint
Gaudi::Property< bool > m_firstMeasuredPoint
Definition: TrkVKalVrtFitter.h:341
cfPEst.h
pow
constexpr int pow(int base, int exp) noexcept
Definition: ap_fixedTest.cxx:15
Trk::VKalVrtControl::vk_forcft
ForCFT vk_forcft
Definition: TrkVKalVrtCore.h:92
python.AutoConfigFlags.msg
msg
Definition: AutoConfigFlags.py:7
Trk::ForCFT::robres
double robres[vkalNTrkM]
Definition: ForCFT.h:51
Trk::TrkVKalVrtFitter::State::m_usePointingCnst
bool m_usePointingCnst
Definition: TrkVKalVrtFitter.h:430
Trk::TrkVKalVrtFitter::State::m_apar
double m_apar[NTrMaxVFit][5]
Definition: TrkVKalVrtFitter.h:401
Trk::TrkVKalVrtFitter::State::m_ErrMtx
std::vector< double > m_ErrMtx
Definition: TrkVKalVrtFitter.h:422
Trk::v
@ v
Definition: ParamDefs.h:78
Trk::TrkVKalVrtFitter::m_IDsizeZ
Gaudi::Property< double > m_IDsizeZ
Definition: TrkVKalVrtFitter.h:327