ATLAS Offline Software
TrkV0VertexFitter.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2025 CERN for the benefit of the ATLAS collaboration
3 */
4 
5 /***************************************************************************
6  TrkV0VertexFitter.cxx - Description
7  ***************************************************************************/
8 #include "TrkV0Fitter/TrkV0VertexFitter.h"
9 #include "TrkDetDescrUtils/GeometryStatics.h"
10 #include "TrkExInterfaces/IExtrapolator.h"
11 #include "TrkParameters/TrackParameters.h"
12 #include "TrkSurfaces/CylinderSurface.h"
13 #include "TrkSurfaces/PerigeeSurface.h"
14 #include "VxVertex/VxTrackAtVertex.h"
15 #include "xAODTracking/TrackParticle.h"
16 #include "xAODTracking/TrackParticleContainer.h"
17 #include "xAODTracking/Vertex.h"
18 
19 #include "StoreGate/ReadCondHandle.h"
20 
21 /* These are some local helper classes only needed for convenience, therefore
22 within anonymous namespace. They contain temporary calculations of matrices
23 and vectors resulting from the vertex calculation. */
24 namespace
25 {
26  struct V0FitterTrack final
27  {
28  V0FitterTrack() : originalPerigee(nullptr), chi2(-1.) {}
29  ~V0FitterTrack() = default;
30  const Trk::TrackParameters * originalPerigee;
31  double chi2;
32  AmgVector(5) TrkPar;
33  AmgSymMatrix(5) Wi_mat;
34  };
35 }
36 
37 namespace Trk
38 {
39  TrkV0VertexFitter::TrkV0VertexFitter(const std::string& t, const std::string& n, const IInterface* p) : base_class(t,n,p),
40  m_maxIterations(10),
41  m_maxDchi2PerNdf(0.1),
42  m_maxR(2000.),
43  m_maxZ(5000.),
44  m_firstMeas(true),
45  m_deltaR(false),
46  m_extrapolator("Trk::Extrapolator/InDetExtrapolator", this)
47  {
48  declareProperty("MaxIterations", m_maxIterations);
49  declareProperty("MaxChi2PerNdf", m_maxDchi2PerNdf);
50  declareProperty("MaxR", m_maxR);
51  declareProperty("MaxZ", m_maxZ);
52  declareProperty("FirstMeasuredPoint", m_firstMeas);
53  declareProperty("Use_deltaR", m_deltaR);
54  declareProperty("Extrapolator", m_extrapolator);
55  declareInterface<IVertexFitter>(this);
56  }
57 
58  TrkV0VertexFitter::~TrkV0VertexFitter() = default;
59 
60  StatusCode TrkV0VertexFitter::initialize()
61  {
62  if ( m_extrapolator.retrieve().isFailure() ) {
63  ATH_MSG_FATAL("Failed to retrieve tool " << m_extrapolator);
64  return StatusCode::FAILURE;
65  }
66  ATH_MSG_DEBUG( "Retrieved tool " << m_extrapolator );
67 
68 
69  ATH_CHECK( m_fieldCacheCondObjInputKey.initialize() );
70 
71  ATH_MSG_DEBUG( "Initialize successful");
72  return StatusCode::SUCCESS;
73  }
74 
75  StatusCode TrkV0VertexFitter::finalize()
76  {
77  ATH_MSG_DEBUG( "Finalize successful" );
78  return StatusCode::SUCCESS;
79  }
80 
81 
83  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const xAOD::TrackParticle*>& vectorTrk,
84  const Amg::Vector3D& firstStartingPoint) const
85  {
86  std::vector<double> masses;
87  double constraintMass = -9999.;
88  xAOD::Vertex * pointingVertex = nullptr;
89  return fit(vectorTrk, masses, constraintMass, pointingVertex, firstStartingPoint);
90  }
91 
93  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const xAOD::TrackParticle*>& vectorTrk,
94  const xAOD::Vertex& firstStartingPoint) const
95  {
96  std::vector<double> masses;
97  double constraintMass = -9999.;
98  xAOD::Vertex * pointingVertex = nullptr;
99  const Amg::Vector3D& startingPoint = firstStartingPoint.position();
100  return fit(vectorTrk, masses, constraintMass, pointingVertex, startingPoint);
101  }
102 
104  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const xAOD::TrackParticle*>& vectorTrk) const
105  {
106  Amg::Vector3D tmpVtx;
107  tmpVtx.setZero();
108  return fit(vectorTrk, tmpVtx);
109  }
110 
112  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const xAOD::TrackParticle*> & vectorTrk,
113  const std::vector<double>& masses,
114  const double& constraintMass,
115  const xAOD::Vertex* pointingVertex,
116  const Amg::Vector3D& firstStartingPoint) const
117  {
118  const EventContext& ctx = Gaudi::Hive::currentContext();
119  std::vector<const Trk::TrackParameters*> measuredPerigees;
120  std::vector<const Trk::TrackParameters*> measuredPerigees_delete;
121  for (const xAOD::TrackParticle* p : vectorTrk)
122  {
123  if (m_firstMeas) {
124  unsigned int indexFMP;
125  if (p->indexOfParameterAtPosition(indexFMP, xAOD::FirstMeasurement)) {
126  measuredPerigees.push_back(new CurvilinearParameters(p->curvilinearParameters(indexFMP)));
127  ATH_MSG_DEBUG("first measurement on track exists");
128  ATH_MSG_DEBUG("first measurement " << p->curvilinearParameters(indexFMP));
129  ATH_MSG_DEBUG("first measurement covariance " << *(p->curvilinearParameters(indexFMP)).covariance());
130  } else {
131  Amg::Transform3D CylTrf;
132  CylTrf.setIdentity();
133  Trk::CylinderSurface estimationCylinder(CylTrf, p->radiusOfFirstHit(), 10e10);
134  const Trk::TrackParameters* chargeParameters = &p->perigeeParameters();
135  MaterialUpdateMode mode = Trk::removeNoise;
136 
137  const Trk::TrackParameters* extrapolatedPerigee =
138  std::abs(chargeParameters->position().z()) > m_maxZ ? nullptr :
139  m_extrapolator->extrapolate(ctx,
140  *chargeParameters,
141  estimationCylinder,
142  Trk::alongMomentum,
143  true,
144  Trk::pion,
145  mode).release();
146 
147  if (extrapolatedPerigee != nullptr) {
148  ATH_MSG_DEBUG("extrapolated to first measurement");
149  measuredPerigees.push_back (extrapolatedPerigee);
150  measuredPerigees_delete.push_back (extrapolatedPerigee);
151  } else {
152 
153  extrapolatedPerigee =
154  std::abs(chargeParameters->position().z()) > m_maxZ ? nullptr :
155  m_extrapolator->extrapolateDirectly(ctx,
156  *chargeParameters,
157  estimationCylinder,
158  Trk::alongMomentum,
159  true,
160  Trk::pion).release();
161 
162  if (extrapolatedPerigee != nullptr) {
163  ATH_MSG_DEBUG( "extrapolated (direct) to first measurement");
164  measuredPerigees.push_back (extrapolatedPerigee);
165  measuredPerigees_delete.push_back (extrapolatedPerigee);
166  } else {
167  ATH_MSG_DEBUG("Failed to extrapolate to the first measurement on track, using Perigee parameters");
168  measuredPerigees.push_back (&p->perigeeParameters());
169  }
170  }
171  }
172  } else {
173  measuredPerigees.push_back (&p->perigeeParameters());
174  }
175  }
176 
177  xAOD::Vertex * fittedVxCandidate = fit(measuredPerigees, masses, constraintMass, pointingVertex, firstStartingPoint);
178 
179  // assign the used tracks to the V0Candidate
180  if (fittedVxCandidate) {
181  for (const xAOD::TrackParticle* p : vectorTrk)
182  {
183  ElementLink<xAOD::TrackParticleContainer> el;
184  el.setElement(p);
185  fittedVxCandidate->addTrackAtVertex (el);
186  }
187  }
188 
189  for (const auto *ptr : measuredPerigees_delete){ delete ptr; }
190 
191  return fittedVxCandidate;
192  }
193 
194 
195 
197  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const Trk::TrackParameters*> & originalPerigees,
198  const Amg::Vector3D& firstStartingPoint) const
199  {
200  std::vector<double> masses;
201  double constraintMass = -9999.;
202  xAOD::Vertex * pointingVertex = nullptr;
203  return fit(originalPerigees, masses, constraintMass, pointingVertex, firstStartingPoint);
204  }
205 
207  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const Trk::TrackParameters*> & originalPerigees,
208  const xAOD::Vertex& firstStartingPoint) const
209  {
210  std::vector<double> masses;
211  double constraintMass = -9999.;
212  xAOD::Vertex * pointingVertex = nullptr;
213  const Amg::Vector3D& startingPoint = firstStartingPoint.position();
214  return fit(originalPerigees, masses, constraintMass, pointingVertex, startingPoint);
215  }
216 
218  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const Trk::TrackParameters*>& originalPerigees) const
219  {
220  Amg::Vector3D tmpVtx;
221  tmpVtx.setZero();
222  return fit(originalPerigees, tmpVtx);
223  }
224 
226  xAOD::Vertex * TrkV0VertexFitter::fit(const std::vector<const Trk::TrackParameters*>& originalPerigees,
227  const std::vector<double>& masses,
228  const double& constraintMass,
229  const xAOD::Vertex* pointingVertex,
230  const Amg::Vector3D& firstStartingPoint) const
231  {
232  const EventContext& ctx = Gaudi::Hive::currentContext();
233  if ( originalPerigees.empty() )
234  {
235  ATH_MSG_DEBUG("No tracks to fit in this event.");
236  return nullptr;
237  }
238 
239  // Initialisation of variables
240  bool pointingConstraint = false;
241  bool massConstraint = false;
242  if(constraintMass > -100.) massConstraint = true;
243  bool conversion = false;
244  if(constraintMass == 0. && originalPerigees.size() == 2) conversion = true;
245  double x_point=0., y_point=0., z_point=0.;
246  AmgSymMatrix(3) pointingVertexCov; pointingVertexCov.setIdentity();
247  if (pointingVertex != nullptr) {
248  if (pointingVertex->covariancePosition().trace() != 0.) {
249  pointingConstraint = true;
250  Amg::Vector3D pv = pointingVertex->position();
251  x_point = pv.x();
252  y_point = pv.y();
253  z_point = pv.z();
254  pointingVertexCov = pointingVertex->covariancePosition().inverse();
255  }
256  }
257 
258  if (msgLvl(MSG::DEBUG)) {
259  msg(MSG::DEBUG) << "massConstraint " << massConstraint << " pointingConstraint " << pointingConstraint << " conversion " << conversion << endmsg;
260  msg(MSG::DEBUG) << "V0Fitter called with: " << endmsg;
261  if (massConstraint && !masses.empty()) msg(MSG::DEBUG) << "mass constraint, V0Mass = " << constraintMass << " particle masses " << masses << endmsg;
262  if (pointingConstraint) msg(MSG::DEBUG) << "pointing constraint, x = " << x_point << " y = " << y_point << " z = " << z_point << endmsg;
263  }
264 
265  bool restartFit = true;
266  double chi2 = 2000000000000.;
267  unsigned int nTrk = originalPerigees.size(); // Number of tracks to fit
268  unsigned int nMeas = 5*nTrk; // Number of measurements
269  unsigned int nVert = 1; // Number of vertices
270 
271  unsigned int nCnst = 2*nTrk; // Number of constraint equations
272  unsigned int nPntC = 2; // Contribution from pointing constraint in 2D
273  unsigned int nMass = 1; // Contribution from mass constraint
274 
275  if (massConstraint) {
276  nCnst = nCnst + nMass;
277  }
278  if (pointingConstraint) {
279  nCnst = nCnst + nPntC;
280  nMeas = nMeas + 3;
281  nVert = nVert + 1;
282  }
283 
284  unsigned int nPar = 5*nTrk + 3*nVert; // Number of parameters
285  int ndf = nMeas - (nPar - nCnst); // Number of degrees of freedom
286  if (ndf < 0) {ndf = 1;}
287 
288  unsigned int dim = nCnst; //
289  unsigned int n_dim = nMeas; //
290 
291  ATH_MSG_DEBUG("ndf " << ndf << " n_dim " << n_dim << " dim " << dim);
292 
293  std::vector<V0FitterTrack> v0FitterTracks;
294 
295  Amg::VectorX Y_vec(n_dim); Y_vec.setZero();
296  Amg::VectorX Y0_vec(n_dim); Y0_vec.setZero();
297  Amg::Vector3D A_vec; A_vec.setZero();
298 
299  Amg::MatrixX Wmeas_mat(n_dim,n_dim); Wmeas_mat.setZero();
300  Amg::MatrixX Wmeas0_mat(n_dim,n_dim); Wmeas0_mat.setZero();
301  Amg::MatrixX Bjac_mat(dim,n_dim); Bjac_mat.setZero();
302  Amg::MatrixX Ajac_mat(dim,3); Ajac_mat.setZero();
303  Amg::MatrixX C11_mat(n_dim,n_dim); C11_mat.setZero();
304  Amg::MatrixX C22_mat(3,3); C22_mat.setZero();
305  Amg::MatrixX C21_mat(3,n_dim); C21_mat.setZero();
306  Amg::MatrixX C31_mat(dim,n_dim); C31_mat.setZero();
307  Amg::MatrixX C32_mat(dim,3); C32_mat.setZero();
308  Amg::MatrixX Wb_mat(dim,dim); Wb_mat.setZero();
309  Amg::MatrixX Btemp_mat(dim,n_dim); Btemp_mat.setZero();
310  Amg::MatrixX Atemp_mat(dim,3); Atemp_mat.setZero();
311  Amg::VectorX DeltaY_vec(n_dim); DeltaY_vec.setZero();
312  Amg::Vector3D DeltaA_vec; DeltaA_vec.setZero();
313  Amg::VectorX DeltaY0_vec(n_dim); DeltaY0_vec.setZero();
314  Amg::VectorX F_vec(dim); F_vec.setZero();
315  Amg::VectorX C_vec(dim); C_vec.setZero();
316  Amg::VectorX C_cor_vec(dim); C_cor_vec.setZero();
317  Amg::MatrixX V_mat(nPar,nPar); V_mat.setZero();
318  Amg::MatrixX Chi_vec(1,n_dim); Chi_vec.setZero();
319  AmgSymMatrix(1) Chi_mat; Chi_mat.setZero();
320  Amg::MatrixX ChiItr_vec(1,n_dim); ChiItr_vec.setZero();
321  AmgSymMatrix(1) ChiItr_mat; ChiItr_mat.setZero();
322  Amg::VectorX F_fac_vec(dim); F_fac_vec.setZero();
323 
324  const Amg::Vector3D * globalPosition = &(firstStartingPoint);
325  ATH_MSG_DEBUG("globalPosition of starting point: " << (*globalPosition)[0] << ", " << (*globalPosition)[1] << ", " << (*globalPosition)[2]);
326 
327  if (globalPosition->perp() > m_maxR && globalPosition->z() > m_maxZ) return nullptr;
328 
329  SG::ReadCondHandle<AtlasFieldCacheCondObj> readHandle{m_fieldCacheCondObjInputKey, ctx};
330  if (!readHandle.isValid()) {
331  std::string msg = "Failed to retrieve magmnetic field conditions data ";
332  msg += m_fieldCacheCondObjInputKey.key();
333  throw std::runtime_error(msg);
334  }
335  const AtlasFieldCacheCondObj* fieldCondObj{*readHandle};
336 
337  MagField::AtlasFieldCache fieldCache;
338  fieldCondObj->getInitializedCache (fieldCache);
339 
340  // magnetic field
341  double BField[3];
342  fieldCache.getField(globalPosition->data(),BField);
343  double B_z = BField[2]*299.792; // should be in GeV/mm
344  if (B_z == 0. || std::isnan(B_z)) {
345  ATH_MSG_DEBUG("Could not find a magnetic field different from zero: very very strange");
346  B_z = 0.60407; // Value in GeV/mm (ATLAS units)
347  } else {
348  ATH_MSG_VERBOSE("Magnetic field projection of z axis in the perigee position is: " << B_z << " GeV/mm ");
349  }
350 // double B_z = 1.998*0.3;
351 
352 
353  v0FitterTracks.clear();
354  Trk::PerigeeSurface perigeeSurface(*globalPosition);
355  // Extrapolate the perigees to the startpoint of the fit
356  for (const Trk::TrackParameters* chargeParameters : originalPerigees)
357  {
358  if (chargeParameters != nullptr)
359  {
360  // Correct material changes
361  const Amg::Vector3D gMomentum = chargeParameters->momentum();
362  const Amg::Vector3D gDirection = chargeParameters->position() - *globalPosition;
363  const double extrapolationDirection = gMomentum.dot( gDirection );
364  MaterialUpdateMode mode = Trk::removeNoise;
365  if(extrapolationDirection > 0) mode = Trk::addNoise;
366  std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(nullptr);
367 
368  std::unique_ptr<const Trk::TrackParameters> tmp =
369  std::abs(chargeParameters->position().z()) > m_maxZ ? nullptr :
370  m_extrapolator->extrapolate(ctx,
371  *chargeParameters,
372  perigeeSurface,
373  Trk::anyDirection,
374  true,
375  Trk::pion,
376  mode);
377 
378  //if of right type we want to pass ownership
379  if (tmp && tmp->associatedSurface().type() == Trk::SurfaceType::Perigee) {
380  extrapolatedPerigee.reset(static_cast<const Trk::Perigee*>(tmp.release()));
381  }
382 
383  if (extrapolatedPerigee == nullptr) {
384  ATH_MSG_DEBUG("Perigee was not extrapolated! Taking original one!");
385  const Trk::Perigee* tmpPerigee = dynamic_cast<const Trk::Perigee*>(chargeParameters);
386  if (tmpPerigee!=nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
387  else return nullptr;
388  }
389 
390  // store track parameters at starting point
391  V0FitterTrack locV0FitterTrack;
392  locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[Trk::d0];
393  locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[Trk::z0];
394  locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[Trk::phi];
395  locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[Trk::theta];
396  locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[Trk::qOverP];
397  locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
398  locV0FitterTrack.originalPerigee = chargeParameters;
399  v0FitterTracks.push_back(locV0FitterTrack);
400  } else {
401  ATH_MSG_DEBUG("Track parameters are not charged tracks ... fit aborted");
402  return nullptr;
403  }
404  }
405 
406  // Iterate fits until the fit criteria are met, or the number of max iterations is reached
407  double chi2New=0.; double chi2Old=chi2;
408  double sumConstr=0.;
409  bool onConstr = false;
410  Amg::Vector3D frameOrigin = firstStartingPoint;
411  Amg::Vector3D frameOriginItr = firstStartingPoint;
412  for (int itr=0; itr < m_maxIterations; ++itr)
413  {
414  ATH_MSG_DEBUG("Iteration number: " << itr);
415  if (!restartFit) chi2Old = chi2New;
416  chi2New = 0.;
417 
418  if (restartFit)
419  {
420  // ===> loop over tracks
421  std::vector<V0FitterTrack>::iterator PTIter;
422  int i=0;
423  for (PTIter = v0FitterTracks.begin(); PTIter != v0FitterTracks.end() ; ++PTIter)
424  {
425  V0FitterTrack locP((*PTIter));
426  Wmeas0_mat.block(5*i,5*i,5,5) = locP.Wi_mat;
427  Wmeas_mat.block(5*i,5*i,5,5) = locP.Wi_mat;
428  for (int j=0; j<5; ++j) {
429  Y0_vec(j+5*i) = locP.TrkPar[j];
430  }
431  ++i;
432  }
433  if(pointingConstraint) {
434  Y0_vec(5*nTrk + 0) = x_point;
435  Y0_vec(5*nTrk + 1) = y_point;
436  Y0_vec(5*nTrk + 2) = z_point;
437  Wmeas0_mat.block(5*nTrk,5*nTrk,3,3) = pointingVertexCov;
438  Wmeas_mat.block(5*nTrk,5*nTrk,3,3) = pointingVertexCov;
439  }
440  Wmeas_mat = Wmeas_mat.inverse();
441  }
442 
443  Y_vec = Y0_vec + DeltaY_vec;
444  A_vec = DeltaA_vec;
445 
446  // check theta and phi ranges
447  for (unsigned int i=0; i<nTrk; ++i)
448  {
449  if ( fabs ( Y_vec(2+5*i) ) > 100. || fabs ( Y_vec(3+5*i) ) > 100. ) { return nullptr; }
450  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;
451  while ( Y_vec(3+5*i) > 2*M_PI ) Y_vec(3+5*i) -= 2*M_PI;
452  while ( Y_vec(3+5*i) < -M_PI ) Y_vec(3+5*i) += M_PI;
453  if ( Y_vec(3+5*i) > M_PI )
454  {
455  Y_vec(3+5*i) = 2*M_PI - Y_vec(3+5*i);
456  if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -M_PI : M_PI;
457  }
458  if ( Y_vec(3+5*i) < 0.0 )
459  {
460  Y_vec(3+5*i) = - Y_vec(3+5*i);
461  if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -M_PI : M_PI;
462  }
463  }
464 
465  double SigE=0., SigPx=0., SigPy=0., SigPz=0., Px=0., Py=0., Pz=0.;
466  Amg::VectorX rho(nTrk), Phi(nTrk), charge(nTrk);
467  rho.setZero(); Phi.setZero(); charge.setZero();
468  Amg::VectorX d0Cor(nTrk), d0Fac(nTrk), xcphiplusysphi(nTrk), xsphiminusycphi(nTrk);
469  d0Cor.setZero(); d0Fac.setZero(); xcphiplusysphi.setZero(); xsphiminusycphi.setZero();
470  AmgVector(2) conv_sign;
471  conv_sign[0] = -1; conv_sign[1] = 1;
472  for (unsigned int i=0; i<nTrk; ++i)
473  {
474  charge[i] = (Y_vec(4+5*i) < 0.) ? -1. : 1.;
475  rho[i] = sin(Y_vec(3+5*i))/(B_z*Y_vec(4+5*i));
476  xcphiplusysphi[i] = A_vec(0)*cos(Y_vec(2+5*i))+A_vec(1)*sin(Y_vec(2+5*i));
477  xsphiminusycphi[i] = A_vec(0)*sin(Y_vec(2+5*i))-A_vec(1)*cos(Y_vec(2+5*i));
478  if(fabs(-xcphiplusysphi[i]/rho[i]) > 1.) return nullptr;
479  d0Cor[i] = 0.5*asin(-xcphiplusysphi[i]/rho[i]);
480  double d0Facsq = 1. - xcphiplusysphi[i]*xcphiplusysphi[i]/(rho[i]*rho[i]);
481  d0Fac[i] = (d0Facsq>0.) ? sqrt(d0Facsq) : 0;
482  Phi[i] = Y_vec(2+5*i) + 2.*d0Cor[i];
483 
484  if(massConstraint && !masses.empty() && masses[i] != 0.){
485  SigE += sqrt(1./(Y_vec(4+5*i)*Y_vec(4+5*i)) + masses[i]*masses[i]);
486  SigPx += sin(Y_vec(3+5*i))*cos(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
487  SigPy += sin(Y_vec(3+5*i))*sin(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
488  SigPz += cos(Y_vec(3+5*i))*charge[i]/Y_vec(4+5*i);
489  }
490  Px += sin(Y_vec(3+5*i))*cos(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
491  Py += sin(Y_vec(3+5*i))*sin(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
492  Pz += cos(Y_vec(3+5*i))*charge[i]/Y_vec(4+5*i);
493  }
494 
495  double FMass=0., dFMassdxs=0., dFMassdys=0., dFMassdzs=0.;
496  double FPxy=0., dFPxydxs=0., dFPxydys=0., dFPxydzs=0., dFPxydxp=0., dFPxydyp=0., dFPxydzp=0.;
497  double FPxz=0., dFPxzdxs=0., dFPxzdys=0., dFPxzdzs=0., dFPxzdxp=0., dFPxzdyp=0., dFPxzdzp=0.;
498  Amg::VectorX Fxy(nTrk), Fxz(nTrk), dFMassdPhi(nTrk);
499  Fxy.setZero(); Fxz.setZero(); dFMassdPhi.setZero();
500  Amg::VectorX drhodtheta(nTrk), drhodqOverP(nTrk), csplusbc(nTrk), ccminusbs(nTrk);
501  drhodtheta.setZero(); drhodqOverP.setZero(); csplusbc.setZero(); ccminusbs.setZero();
502  Amg::VectorX dFxydd0(nTrk), dFxydz0(nTrk), dFxydphi(nTrk), dFxydtheta(nTrk), dFxydqOverP(nTrk);
503  dFxydd0.setZero(); dFxydz0.setZero(); dFxydphi.setZero(); dFxydtheta.setZero(); dFxydqOverP.setZero();
504  Amg::VectorX dFxydxs(nTrk), dFxydys(nTrk), dFxydzs(nTrk);
505  dFxydxs.setZero(); dFxydys.setZero(); dFxydzs.setZero();
506  Amg::VectorX dFxzdd0(nTrk), dFxzdz0(nTrk), dFxzdphi(nTrk), dFxzdtheta(nTrk), dFxzdqOverP(nTrk);
507  dFxzdd0.setZero(); dFxzdz0.setZero(); dFxzdphi.setZero(); dFxzdtheta.setZero(); dFxzdqOverP.setZero();
508  Amg::VectorX dFxzdxs(nTrk), dFxzdys(nTrk), dFxzdzs(nTrk);
509  dFxzdxs.setZero(); dFxzdys.setZero(); dFxzdzs.setZero();
510  Amg::VectorX dFMassdd0(nTrk), dFMassdz0(nTrk), dFMassdphi(nTrk), dFMassdtheta(nTrk), dFMassdqOverP(nTrk);
511  dFMassdd0.setZero(); dFMassdz0.setZero(); dFMassdphi.setZero(); dFMassdtheta.setZero(); dFMassdqOverP.setZero();
512  Amg::VectorX dFPxydd0(nTrk), dFPxydz0(nTrk), dFPxydphi(nTrk), dFPxydtheta(nTrk), dFPxydqOverP(nTrk);
513  dFPxydd0.setZero(); dFPxydz0.setZero(); dFPxydphi.setZero(); dFPxydtheta.setZero(); dFPxydqOverP.setZero();
514  Amg::VectorX dFPxzdd0(nTrk), dFPxzdz0(nTrk), dFPxzdphi(nTrk), dFPxzdtheta(nTrk), dFPxzdqOverP(nTrk);
515  dFPxzdd0.setZero(); dFPxzdz0.setZero(); dFPxzdphi.setZero(); dFPxzdtheta.setZero(); dFPxzdqOverP.setZero();
516  Amg::VectorX dPhidd0(nTrk), dPhidz0(nTrk), dPhidphi0(nTrk), dPhidtheta(nTrk), dPhidqOverP(nTrk);
517  dPhidd0.setZero(); dPhidz0.setZero(); dPhidphi0.setZero(); dPhidtheta.setZero(); dPhidqOverP.setZero();
518  Amg::VectorX dPhidxs(nTrk), dPhidys(nTrk), dPhidzs(nTrk);
519  dPhidxs.setZero(); dPhidys.setZero(); dPhidzs.setZero();
520  //
521  // constraint equations for V0vertex fitter
522  //
523  // FMass = mass vertex constraint
524  //
525  if (conversion) {
526  FMass = Phi[1] - Phi[0];
527  } else {
528  FMass = constraintMass*constraintMass - SigE*SigE + SigPx*SigPx + SigPy*SigPy + SigPz*SigPz;
529  }
530  //
531  // FPxy = pointing constraint in xy
532  //
533  FPxy = Px*(frameOriginItr[1] - y_point) - Py*(frameOriginItr[0]- x_point);
534  //
535  // FPxz = pointing constraint in xz
536  //
537  FPxz = Px*(frameOriginItr[2] - z_point) - Pz*(frameOriginItr[0]- x_point);
538 
539  for (unsigned int i=0; i<nTrk; ++i)
540  {
541  //
542  // Fxy = vertex constraint in xy plane (one for each track)
543  //
544  Fxy[i] = Y_vec(0+5*i) + xsphiminusycphi[i] - 2.*rho[i]*sin(d0Cor[i])*sin(d0Cor[i]);
545  //
546  // Fxz = vertex constraint in xz plane (one for each track)
547  //
548  Fxz[i] = Y_vec(1+5*i) - A_vec(2) - rho[i]*2.*d0Cor[i]/tan(Y_vec(3+5*i));
549  //
550  // derivatives
551  //
552  drhodtheta[i] = cos(Y_vec(3+5*i))/(B_z*Y_vec(4+5*i));
553  drhodqOverP[i] = -sin(Y_vec(3+5*i))/(B_z*Y_vec(4+5*i)*Y_vec(4+5*i));
554 
555  dFxydd0[i] = 1.;
556  dFxydphi[i] = xcphiplusysphi[i]*(1. + xsphiminusycphi[i]/(d0Fac[i]*rho[i]));
557  dFxydtheta[i] = (xcphiplusysphi[i]*xcphiplusysphi[i]/(d0Fac[i]*rho[i]*rho[i])-2.*sin(d0Cor[i])*sin(d0Cor[i]))*drhodtheta[i];
558  dFxydqOverP[i] = (xcphiplusysphi[i]*xcphiplusysphi[i]/(d0Fac[i]*rho[i]*rho[i])-2.*sin(d0Cor[i])*sin(d0Cor[i]))*drhodqOverP[i];
559  dFxydxs[i] = sin(Y_vec(2+5*i)) - cos(Y_vec(2+5*i))*xcphiplusysphi[i]/(d0Fac[i]*rho[i]);
560  dFxydys[i] = -cos(Y_vec(2+5*i)) - sin(Y_vec(2+5*i))*xcphiplusysphi[i]/(d0Fac[i]*rho[i]);
561 
562  dFxzdz0[i] = 1.;
563  dFxzdphi[i] = -xsphiminusycphi[i]/(d0Fac[i]*tan(Y_vec(3+5*i)));
564  dFxzdtheta[i] = -((xcphiplusysphi[i]/(d0Fac[i]*rho[i]) + 2.*d0Cor[i])*tan(Y_vec(3+5*i))*drhodtheta[i] -
565  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)));
566  dFxzdqOverP[i] = -(xcphiplusysphi[i]/(d0Fac[i]*rho[i]) + 2.*d0Cor[i])*drhodqOverP[i]/tan(Y_vec(3+5*i));
567  dFxzdxs[i] = cos(Y_vec(2+5*i))/(d0Fac[i]*tan(Y_vec(3+5*i)));
568  dFxzdys[i] = sin(Y_vec(2+5*i))/(d0Fac[i]*tan(Y_vec(3+5*i)));
569  dFxzdzs[i] = -1.;
570 
571  dPhidphi0[i] = 1. + xsphiminusycphi[i]/(d0Fac[i]*rho[i]);
572  dPhidtheta[i] = xcphiplusysphi[i]*drhodtheta[i]/(d0Fac[i]*rho[i]*rho[i]);
573  dPhidqOverP[i] = xcphiplusysphi[i]*drhodqOverP[i]/(d0Fac[i]*rho[i]*rho[i]);
574  dPhidxs[i] = -cos(Y_vec(2+5*i))/(d0Fac[i]*rho[i]);
575  dPhidys[i] = -sin(Y_vec(2+5*i))/(d0Fac[i]*rho[i]);
576 
577  if (massConstraint && !masses.empty() && masses[i] != 0.){
578  if (conversion) {
579  dFMassdphi[i] = conv_sign[i]*dPhidphi0[i];
580  dFMassdtheta[i] = conv_sign[i]*dPhidtheta[i];
581  dFMassdqOverP[i] = conv_sign[i]*dPhidqOverP[i];
582  dFMassdxs += conv_sign[i]*dPhidxs[i];
583  dFMassdys += conv_sign[i]*dPhidys[i];
584  } else {
585  csplusbc[i] = SigPy*sin(Y_vec(2+5*i))+SigPx*cos(Y_vec(2+5*i));
586  ccminusbs[i] = SigPy*cos(Y_vec(2+5*i))-SigPx*sin(Y_vec(2+5*i));
587  dFMassdphi[i] = 2.*sin(Y_vec(3+5*i))*ccminusbs[i]*charge[i]/Y_vec(4+5*i);
588  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);
589  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)) -
590  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));
591  }
592  }
593 
594  if (pointingConstraint){
595  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);
596  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);
597  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));
598  dFPxydxs += -sin(Y_vec(3+5*i))*sin(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
599  dFPxydys += sin(Y_vec(3+5*i))*cos(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
600  dFPxydxp += sin(Y_vec(3+5*i))*sin(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
601  dFPxydyp += -sin(Y_vec(3+5*i))*cos(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
602 
603  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);
604  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)
605  +sin(Y_vec(3+5*i))*(frameOriginItr[0]-x_point)*charge[i]/Y_vec(4+5*i);
606  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))
607  +cos(Y_vec(3+5*i))*(frameOriginItr[0]-x_point)*charge[i]/(Y_vec(4+5*i)*Y_vec(4+5*i));
608  dFPxzdxs += -cos(Y_vec(3+5*i))*charge[i]/Y_vec(4+5*i);
609  dFPxzdzs += sin(Y_vec(3+5*i))*cos(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
610  dFPxzdxp += cos(Y_vec(3+5*i))*charge[i]/Y_vec(4+5*i);
611  dFPxzdzp += -sin(Y_vec(3+5*i))*cos(Y_vec(2+5*i))*charge[i]/Y_vec(4+5*i);
612  }
613 
614  // fill vector of constraints
615  F_vec[i] = -Fxy[i];
616  F_vec[i+nTrk] = -Fxz[i];
617  F_fac_vec[i] = 1.;
618  F_fac_vec[i+nTrk] = 1.;
619  }
620  if(massConstraint) F_vec(2*nTrk+0) = -FMass;
621  //if(massConstraint) F_fac_vec(2*nTrk+0) = 1.;
622  if(massConstraint) F_fac_vec(2*nTrk+0) = 0.000001;
623  if(pointingConstraint) {
624  if(massConstraint) {
625  F_vec(2*nTrk+1) = -FPxy;
626  F_vec(2*nTrk+2) = -FPxz;
627  F_fac_vec(2*nTrk+1) = 0.000001;
628  F_fac_vec(2*nTrk+2) = 0.000001;
629  } else {
630  F_vec(2*nTrk+0) = -FPxy;
631  F_vec(2*nTrk+1) = -FPxz;
632  F_fac_vec(2*nTrk+0) = 0.000001;
633  F_fac_vec(2*nTrk+1) = 0.000001;
634  }
635  }
636 
637  sumConstr = 0.;
638  for (unsigned int i=0; i<dim; ++i)
639  {
640  sumConstr += F_fac_vec[i]*fabs(F_vec[i]);
641  }
642  if ( std::isnan(sumConstr) ) { return nullptr; }
643  if (sumConstr < 0.001) { onConstr = true; }
644  ATH_MSG_DEBUG("sumConstr " << sumConstr);
645 
646  for (unsigned int i=0; i<nTrk; ++i)
647  {
648  Bjac_mat(i,0+5*i) = dFxydd0(i);
649  Bjac_mat(i,1+5*i) = dFxydz0(i);
650  Bjac_mat(i,2+5*i) = dFxydphi(i);
651  Bjac_mat(i,3+5*i) = dFxydtheta(i);
652  Bjac_mat(i,4+5*i) = dFxydqOverP(i);
653  Bjac_mat(i+nTrk,0+5*i) = dFxzdd0(i);
654  Bjac_mat(i+nTrk,1+5*i) = dFxzdz0(i);
655  Bjac_mat(i+nTrk,2+5*i) = dFxzdphi(i);
656  Bjac_mat(i+nTrk,3+5*i) = dFxzdtheta(i);
657  Bjac_mat(i+nTrk,4+5*i) = dFxzdqOverP(i);
658  if(massConstraint) {
659  Bjac_mat(2*nTrk,0+5*i) = dFMassdd0(i);
660  Bjac_mat(2*nTrk,1+5*i) = dFMassdz0(i);
661  Bjac_mat(2*nTrk,2+5*i) = dFMassdphi(i);
662  Bjac_mat(2*nTrk,3+5*i) = dFMassdtheta(i);
663  Bjac_mat(2*nTrk,4+5*i) = dFMassdqOverP(i);
664  }
665  if(pointingConstraint) {
666  if(massConstraint) {
667  Bjac_mat(2*nTrk+1,0+5*i) = dFPxydd0(i);
668  Bjac_mat(2*nTrk+1,1+5*i) = dFPxydz0(i);
669  Bjac_mat(2*nTrk+1,2+5*i) = dFPxydphi(i);
670  Bjac_mat(2*nTrk+1,3+5*i) = dFPxydtheta(i);
671  Bjac_mat(2*nTrk+1,4+5*i) = dFPxydqOverP(i);
672  Bjac_mat(2*nTrk+1,5*nTrk) = dFPxydxp;
673  Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxydyp;
674  Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxydzp;
675  Bjac_mat(2*nTrk+2,0+5*i) = dFPxzdd0(i);
676  Bjac_mat(2*nTrk+2,1+5*i) = dFPxzdz0(i);
677  Bjac_mat(2*nTrk+2,2+5*i) = dFPxzdphi(i);
678  Bjac_mat(2*nTrk+2,3+5*i) = dFPxzdtheta(i);
679  Bjac_mat(2*nTrk+2,4+5*i) = dFPxzdqOverP(i);
680  Bjac_mat(2*nTrk+2,5*nTrk) = dFPxzdxp;
681  Bjac_mat(2*nTrk+2,5*nTrk+1) = dFPxzdyp;
682  Bjac_mat(2*nTrk+2,5*nTrk+2) = dFPxzdzp;
683  } else {
684  Bjac_mat(2*nTrk+0,0+5*i) = dFPxydd0(i);
685  Bjac_mat(2*nTrk+0,1+5*i) = dFPxydz0(i);
686  Bjac_mat(2*nTrk+0,2+5*i) = dFPxydphi(i);
687  Bjac_mat(2*nTrk+0,3+5*i) = dFPxydtheta(i);
688  Bjac_mat(2*nTrk+0,4+5*i) = dFPxydqOverP(i);
689  Bjac_mat(2*nTrk+0,5*nTrk) = dFPxydxp;
690  Bjac_mat(2*nTrk+0,5*nTrk+1) = dFPxydyp;
691  Bjac_mat(2*nTrk+0,5*nTrk+2) = dFPxydzp;
692  Bjac_mat(2*nTrk+1,0+5*i) = dFPxzdd0(i);
693  Bjac_mat(2*nTrk+1,1+5*i) = dFPxzdz0(i);
694  Bjac_mat(2*nTrk+1,2+5*i) = dFPxzdphi(i);
695  Bjac_mat(2*nTrk+1,3+5*i) = dFPxzdtheta(i);
696  Bjac_mat(2*nTrk+1,4+5*i) = dFPxzdqOverP(i);
697  Bjac_mat(2*nTrk+1,5*nTrk) = dFPxzdxp;
698  Bjac_mat(2*nTrk+1,5*nTrk+1) = dFPxzdyp;
699  Bjac_mat(2*nTrk+1,5*nTrk+2) = dFPxzdzp;
700  }
701  }
702 
703  Ajac_mat(i,0) = dFxydxs(i);
704  Ajac_mat(i,1) = dFxydys(i);
705  Ajac_mat(i,2) = dFxydzs(i);
706  Ajac_mat(i+nTrk,0) = dFxzdxs(i);
707  Ajac_mat(i+nTrk,1) = dFxzdys(i);
708  Ajac_mat(i+nTrk,2) = dFxzdzs(i);
709  if(massConstraint) {
710  Ajac_mat(2*nTrk,0) = dFMassdxs;
711  Ajac_mat(2*nTrk,1) = dFMassdys;
712  Ajac_mat(2*nTrk,2) = dFMassdzs;
713  }
714  if(pointingConstraint) {
715  if(massConstraint) {
716  Ajac_mat(2*nTrk+1,0) = dFPxydxs;
717  Ajac_mat(2*nTrk+1,1) = dFPxydys;
718  Ajac_mat(2*nTrk+1,2) = dFPxydzs;
719  Ajac_mat(2*nTrk+2,0) = dFPxzdxs;
720  Ajac_mat(2*nTrk+2,1) = dFPxzdys;
721  Ajac_mat(2*nTrk+2,2) = dFPxzdzs;
722  } else {
723  Ajac_mat(2*nTrk+0,0) = dFPxydxs;
724  Ajac_mat(2*nTrk+0,1) = dFPxydys;
725  Ajac_mat(2*nTrk+0,2) = dFPxydzs;
726  Ajac_mat(2*nTrk+1,0) = dFPxzdxs;
727  Ajac_mat(2*nTrk+1,1) = dFPxzdys;
728  Ajac_mat(2*nTrk+1,2) = dFPxzdzs;
729  }
730  }
731  }
732 
733  Wb_mat = Wmeas_mat.similarity(Bjac_mat) ;
734  Wb_mat = Wb_mat.inverse();
735 
736  C22_mat = Wb_mat.similarity(Ajac_mat.transpose());
737  C22_mat = C22_mat.inverse();
738 
739  Btemp_mat = Wb_mat * Bjac_mat * Wmeas_mat;
740  Atemp_mat = Wb_mat * Ajac_mat;
741 
742  C21_mat = - C22_mat * Ajac_mat.transpose() * Btemp_mat;
743  C32_mat = Atemp_mat * C22_mat;
744  C31_mat = Btemp_mat + Atemp_mat * C21_mat;
745  Amg::MatrixX mat_prod_1 = Wmeas_mat * Bjac_mat.transpose();
746  Amg::MatrixX mat_prod_2 = Wmeas_mat * Bjac_mat.transpose() * Wb_mat * Ajac_mat;
747  C11_mat = Wmeas_mat - Wb_mat.similarity( mat_prod_1 ) + C22_mat.similarity( mat_prod_2 );
748 
749  C_cor_vec = Ajac_mat*DeltaA_vec + Bjac_mat*DeltaY_vec;
750  C_vec = C_cor_vec + F_vec;
751 
752  DeltaY_vec = C31_mat.transpose()*C_vec;
753  DeltaA_vec = C32_mat.transpose()*C_vec;
754 
755  for (unsigned int i=0; i<n_dim; ++i)
756  {
757  ChiItr_vec(0,i) = DeltaY_vec(i);
758  }
759  ChiItr_mat = Wmeas0_mat.similarity( ChiItr_vec );
760  chi2New = ChiItr_mat(0,0);
761 
762  // current vertex position in global coordinates
763  frameOriginItr[0] += DeltaA_vec(0);
764  frameOriginItr[1] += DeltaA_vec(1);
765  frameOriginItr[2] += DeltaA_vec(2);
766  if (msgLvl(MSG::DEBUG)) {
767  msg(MSG::DEBUG) << "New vertex, global coordinates: " << frameOriginItr.transpose() << endmsg;
768  msg(MSG::DEBUG) << "chi2Old: " << chi2Old << " chi2New: " << chi2New << " fabs(chi2Old-chi2New): " << fabs(chi2Old-chi2New) << endmsg;
769  }
770 
771  const Amg::Vector3D * globalPositionItr = &frameOriginItr;
772  if (globalPositionItr->perp() > m_maxR && globalPositionItr->z() > m_maxZ) return nullptr;
773 
774  if (onConstr && fabs(chi2Old-chi2New) < 0.1) { break; }
775 
776  double BFieldItr[3];
777  fieldCache.getField(globalPositionItr->data(),BFieldItr);
778  double B_z_new = BFieldItr[2]*299.792; // should be in GeV/mm
779  if (B_z_new == 0. || std::isnan(B_z)) {
780  ATH_MSG_DEBUG("Using old B_z");
781  B_z_new = B_z;
782  }
783 
784  restartFit = false;
785  double deltaR = sqrt(DeltaA_vec(0)*DeltaA_vec(0)+DeltaA_vec(1)*DeltaA_vec(1)+DeltaA_vec(2)*DeltaA_vec(2));
786  double deltaB_z = fabs(B_z-B_z_new)/B_z;
787  bool changeBz = false;
788 
789  if (m_deltaR) {
790  if (deltaR > 5. && itr < m_maxIterations-1) changeBz = true;
791  } else {
792  if (deltaB_z > 0.000001 && itr < m_maxIterations-1) changeBz = true;
793  }
794 
795  if (changeBz) {
796  B_z = B_z_new;
797 
798  v0FitterTracks.clear();
799  Trk::PerigeeSurface perigeeSurfaceItr(*globalPositionItr);
800  // Extrapolate the perigees to the new startpoint of the fit
801  for (const Trk::TrackParameters* chargeParameters : originalPerigees)
802  {
803  if (chargeParameters != nullptr)
804  {
805  // Correct material changes
806  const Amg::Vector3D gMomentum = chargeParameters->momentum();
807  const Amg::Vector3D gDirection = chargeParameters->position() - *globalPositionItr;
808  const double extrapolationDirection = gMomentum .dot( gDirection );
809  MaterialUpdateMode mode = Trk::removeNoise;
810  if(extrapolationDirection > 0) mode = Trk::addNoise;
811  std::unique_ptr<const Trk::Perigee> extrapolatedPerigee(nullptr);
812 
813  std::unique_ptr<const Trk::TrackParameters> tmp =
814  std::abs(chargeParameters->position().z()) > m_maxZ ? nullptr :
815  m_extrapolator->extrapolate(ctx,
816  *chargeParameters,
817  perigeeSurfaceItr,
818  Trk::anyDirection,
819  true,
820  Trk::pion,
821  mode);
822 
823  // if of right type we want to pass ownership
824  if (tmp && tmp->associatedSurface().type() == Trk::SurfaceType::Perigee) {
825  extrapolatedPerigee.reset(
826  static_cast<const Trk::Perigee*>(tmp.release()));
827  }
828 
829  if (extrapolatedPerigee == nullptr) {
830  ATH_MSG_DEBUG("Perigee was not extrapolated! Taking original one!");
831  const Trk::Perigee* tmpPerigee = dynamic_cast<const Trk::Perigee*>(chargeParameters);
832  if (tmpPerigee!=nullptr) extrapolatedPerigee = std::make_unique<Trk::Perigee>(*tmpPerigee);
833  else return nullptr;
834  }
835 
836  // store track parameters at new starting point
837  V0FitterTrack locV0FitterTrack;
838  locV0FitterTrack.TrkPar[0] = extrapolatedPerigee->parameters()[Trk::d0];
839  locV0FitterTrack.TrkPar[1] = extrapolatedPerigee->parameters()[Trk::z0];
840  locV0FitterTrack.TrkPar[2] = extrapolatedPerigee->parameters()[Trk::phi];
841  locV0FitterTrack.TrkPar[3] = extrapolatedPerigee->parameters()[Trk::theta];
842  locV0FitterTrack.TrkPar[4] = extrapolatedPerigee->parameters()[Trk::qOverP];
843  locV0FitterTrack.Wi_mat = extrapolatedPerigee->covariance()->inverse().eval();
844  locV0FitterTrack.originalPerigee = chargeParameters;
845  v0FitterTracks.push_back(locV0FitterTrack);
846  } else {
847  ATH_MSG_DEBUG("Track parameters are not charged tracks ... fit aborted");
848  return nullptr;
849  }
850  }
851  frameOrigin = frameOriginItr;
852  Y0_vec *= 0.;
853  Y_vec *= 0.;
854  A_vec *= 0.;
855  DeltaY_vec *= 0.;
856  DeltaA_vec *= 0.;
857  chi2Old = 2000000000000.;
858  chi2New = 0.;
859  sumConstr = 0.;
860  onConstr = false;
861  restartFit = true;
862  }
863 
864  //if (onConstr && fabs(chi2Old-chi2New) < 0.1) { break; }
865 
866  } // end of iteration
867 
868  frameOrigin[0] += DeltaA_vec(0);
869  frameOrigin[1] += DeltaA_vec(1);
870  frameOrigin[2] += DeltaA_vec(2);
871  if ( std::isnan(frameOrigin[0]) || std::isnan(frameOrigin[1]) || std::isnan(frameOrigin[2]) ) return nullptr;
872 
873  Y_vec = Y0_vec + DeltaY_vec;
874 
875  // check theta and phi ranges
876  for (unsigned int i=0; i<nTrk; ++i)
877  {
878  if ( fabs ( Y_vec(2+5*i) ) > 100. || fabs ( Y_vec(3+5*i) ) > 100. ) { return nullptr; }
879  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;
880  while ( Y_vec(3+5*i) > 2*M_PI ) Y_vec(3+5*i) -= 2*M_PI;
881  while ( Y_vec(3+5*i) < -M_PI ) Y_vec(3+5*i) += M_PI;
882  if ( Y_vec(3+5*i) > M_PI )
883  {
884  Y_vec(3+5*i) = 2*M_PI - Y_vec(3+5*i);
885  if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -M_PI : M_PI;
886  }
887  if ( Y_vec(3+5*i) < 0.0 )
888  {
889  Y_vec(3+5*i) = - Y_vec(3+5*i);
890  if ( Y_vec(2+5*i) >= 0 ) Y_vec(2+5*i) += ( Y_vec(2+5*i) >0 ) ? -M_PI : M_PI;
891  }
892  }
893 
894  for (unsigned int i=0; i<n_dim; ++i)
895  {
896  Chi_vec(0,i) = DeltaY_vec(i);
897  }
898  Chi_mat = Wmeas0_mat.similarity( Chi_vec );
899  chi2 = Chi_mat(0,0);
900 
901  V_mat.setZero();
902  V_mat.block(0,0,n_dim,n_dim) = C11_mat;
903  V_mat.block(n_dim,n_dim,3,3) = C22_mat;
904  V_mat.block(n_dim,0,3,n_dim) = C21_mat;
905  V_mat.block(0,n_dim,n_dim,3) = C21_mat.transpose();
906 
907  // ===> loop over tracks
908  std::vector<V0FitterTrack>::iterator BTIter;
909  int iRP=0;
910  for (BTIter = v0FitterTracks.begin(); BTIter != v0FitterTracks.end() ; ++BTIter)
911  {
912  // chi2 per track
913  AmgSymMatrix(5) covTrk; covTrk.setZero();
914  covTrk = Wmeas0_mat.block(5*iRP,5*iRP,4+5*iRP,4+5*iRP);
915  AmgVector(5) chi_vec; chi_vec.setZero();
916  for (unsigned int i=0; i<5; ++i) chi_vec(i) = DeltaY_vec(i+5*iRP);
917  double chi2Trk = chi_vec.dot(covTrk*chi_vec);
918  (*BTIter).chi2=chi2Trk;
919  iRP++;
920  }
921 
922  // Store the vertex
923  xAOD::Vertex* vx = new xAOD::Vertex;
924  vx->makePrivateStore();
925  vx->setPosition (frameOrigin);
926  vx->setCovariancePosition (C22_mat);
927  vx->setFitQuality(chi2,static_cast<float>(ndf));
928  vx->setVertexType(xAOD::VxType::V0Vtx);
929 
930  // Store the tracks at vertex
931  std::vector<VxTrackAtVertex> & tracksAtVertex = vx->vxTrackAtVertex(); tracksAtVertex.clear();
932  Amg::Vector3D Vertex(frameOrigin[0],frameOrigin[1],frameOrigin[2]);
933  const Trk::PerigeeSurface Surface(Vertex);
934  Trk::Perigee * refittedPerigee(nullptr);
935  unsigned int iterf=0;
936  std::vector<V0FitterTrack>::iterator BTIterf;
937  for (BTIterf = v0FitterTracks.begin(); BTIterf != v0FitterTracks.end() ; ++BTIterf)
938  {
939  AmgSymMatrix(5) CovMtxP;
940  CovMtxP.setIdentity();
941  for (unsigned int i=0; i<5; ++i) {
942  for (unsigned int j=0; j<i+1; ++j) {
943  double val = V_mat(5*iterf+i,5*iterf+j);
944  CovMtxP.fillSymmetric(i,j,val);
945  }
946  }
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  // Full Covariance Matrix
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  }
966 
967 
968 } //end of namespace definitions
xAOD::iterator
JetConstituentVector::iterator iterator
Definition: JetConstituentVector.cxx:68
covarianceTool.ndf
ndf
Definition: covarianceTool.py:678
Trk::anyDirection
@ anyDirection
Definition: PropDirection.h:22
Trk::Vertex
Definition: Tracking/TrkEvent/VxVertex/VxVertex/Vertex.h:26
xAOD::Vertex_v1::setPosition
void setPosition(const Amg::Vector3D &position)
Sets the 3-position.
ATH_MSG_FATAL
#define ATH_MSG_FATAL(x)
Definition: AthMsgStreamMacros.h:34
Amg::VectorX
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorX
Dynamic Vector - dynamic allocation.
Definition: EventPrimitives.h:30
xAOD::Vertex_v1::setFitQuality
void setFitQuality(float chiSquared, float numberDoF)
Set the 'Fit Quality' information.
Definition: Vertex_v1.cxx:150
Amg::MatrixX
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
Definition: EventPrimitives.h:27
yodamerge_tmp.dim
dim
Definition: yodamerge_tmp.py:239
TrackParameters.h
xAOD::Vertex
Vertex_v1 Vertex
Define the latest version of the vertex class.
Definition: Event/xAOD/xAODTracking/xAODTracking/Vertex.h:16
SG::ReadCondHandle
Definition: ReadCondHandle.h:44
PerigeeSurface.h
AtlasFieldCacheCondObj
Definition: AtlasFieldCacheCondObj.h:19
Trk::PerigeeSurface
Definition: PerigeeSurface.h:43
Trk::TrkV0VertexFitter::m_deltaR
bool m_deltaR
Definition: TrkV0VertexFitter.h:175
Trk::ParametersBase::position
const Amg::Vector3D & position() const
Access method for the position.
Trk::ParametersT
Dummy class used to allow special convertors to be called for surfaces owned by a detector element.
Definition: EMErrorDetail.h:25
Trk::CurvilinearParameters
CurvilinearParametersT< TrackParametersDim, Charged, PlaneSurface > CurvilinearParameters
Definition: Tracking/TrkEvent/TrkParameters/TrkParameters/TrackParameters.h:29
xAOD::VxType::V0Vtx
@ V0Vtx
Vertex from V0 decay.
Definition: TrackingPrimitives.h:576
M_PI
#define M_PI
Definition: ActiveFraction.h:11
Trk::z0
@ z0
Definition: ParamDefs.h:64
IExtrapolator.h
Trk::MaterialUpdateMode
MaterialUpdateMode
This is a steering enum to force the material update it can be: (1) addNoise (-1) removeNoise Second ...
Definition: MaterialUpdateMode.h:18
Trk::alongMomentum
@ alongMomentum
Definition: PropDirection.h:20
xAOD::Vertex_v1::position
const Amg::Vector3D & position() const
Returns the 3-pos.
Phi
@ Phi
Definition: RPCdef.h:8
read_hist_ntuple.t
t
Definition: read_hist_ntuple.py:5
drawFromPickle.cos
cos
Definition: drawFromPickle.py:36
ATH_MSG_VERBOSE
#define ATH_MSG_VERBOSE(x)
Definition: AthMsgStreamMacros.h:28
SG::VarHandleKey::key
const std::string & key() const
Return the StoreGate ID for the referenced object.
Definition: AthToolSupport/AsgDataHandles/Root/VarHandleKey.cxx:141
dbg::ptr
void * ptr(T *p)
Definition: SGImplSvc.cxx:74
Trk::TrkV0VertexFitter::m_maxR
double m_maxR
Definition: TrkV0VertexFitter.h:172
Trk::Perigee
ParametersT< TrackParametersDim, Charged, PerigeeSurface > Perigee
Definition: Tracking/TrkEvent/TrkParameters/TrkParameters/TrackParameters.h:33
xAOD::Vertex_v1::setCovariance
void setCovariance(const std::vector< float > &value)
Sets the covariance matrix as a simple vector of values.
AmgSymMatrix
#define AmgSymMatrix(dim)
Definition: EventPrimitives.h:50
ReadCondHandle.h
Trk::AmgSymMatrix
AmgSymMatrix(5) &GXFTrackState
Definition: GXFTrackState.h:156
xAOD::Vertex_v1::setVertexType
void setVertexType(VxType::VertexType vType)
Set the type of the vertex.
GeometryStatics.h
xAOD::Vertex_v1::addTrackAtVertex
void addTrackAtVertex(const ElementLink< TrackParticleContainer > &tr, float weight=1.0)
Add a new track to the vertex.
Definition: Vertex_v1.cxx:314
python.utils.AtlRunQueryDQUtils.p
p
Definition: AtlRunQueryDQUtils.py:209
Trk::TrkV0VertexFitter::m_extrapolator
ToolHandle< Trk::IExtrapolator > m_extrapolator
Data members to store the results.
Definition: TrkV0VertexFitter.h:179
lumiFormat.i
int i
Definition: lumiFormat.py:85
beamspotman.n
n
Definition: beamspotman.py:727
Trk::theta
@ theta
Definition: ParamDefs.h:66
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
ATH_MSG_DEBUG
#define ATH_MSG_DEBUG(x)
Definition: AthMsgStreamMacros.h:29
Trk::CylinderSurface
Definition: CylinderSurface.h:55
AmgVector
AmgVector(4) T2BSTrackFilterTool
Definition: T2BSTrackFilterTool.cxx:114
Trk::pion
@ pion
Definition: ParticleHypothesis.h:32
Amg::Transform3D
Eigen::Affine3d Transform3D
Definition: GeoPrimitives.h:46
CylinderSurface.h
chi2
double chi2(TH1 *h0, TH1 *h1)
Definition: comparitor.cxx:525
VxTrackAtVertex.h
Trk::TrkV0VertexFitter::~TrkV0VertexFitter
virtual ~TrkV0VertexFitter()
standard destructor
ATH_CHECK
#define ATH_CHECK
Definition: AthCheckMacros.h:40
Preparation.mode
mode
Definition: Preparation.py:107
python.getProblemFolderFromLogs.el
dictionary el
Definition: getProblemFolderFromLogs.py:48
Trk::TrkV0VertexFitter::m_maxDchi2PerNdf
double m_maxDchi2PerNdf
Definition: TrkV0VertexFitter.h:171
drawFromPickle.tan
tan
Definition: drawFromPickle.py:36
Trk::ParametersBase
Definition: ParametersBase.h:55
DeMoUpdate.tmp
string tmp
Definition: DeMoUpdate.py:1167
Vertex.h
Trk::TrkV0VertexFitter::m_maxZ
double m_maxZ
Definition: TrkV0VertexFitter.h:173
columnar::final
CM final
Definition: ColumnAccessor.h:106
Trk
Ensure that the ATLAS eigen extensions are properly loaded.
Definition: FakeTrackBuilder.h:9
Trk::SurfaceType::Perigee
@ Perigee
Trk::d0
@ d0
Definition: ParamDefs.h:63
charge
double charge(const T &p)
Definition: AtlasPID.h:991
SG::CondHandleKey::initialize
StatusCode initialize(bool used=true)
SG::AuxElement::makePrivateStore
void makePrivateStore()
Create a new (empty) private store for this object.
Definition: AuxElement.cxx:192
Amg::Vector3D
Eigen::Matrix< double, 3, 1 > Vector3D
Definition: GeoPrimitives.h:47
TrkV0VertexFitter.h
TrackParticle.h
Trk::TrkV0VertexFitter::fit
virtual xAOD::Vertex * fit(const std::vector< const xAOD::TrackParticle * > &vectorTrk, const Amg::Vector3D &startingPoint) const override
Interface for xAOD::TrackParticle with Amg::Vector3D starting point.
Definition: TrkV0VertexFitter.cxx:83
xAOD::Vertex_v1
Class describing a Vertex.
Definition: Vertex_v1.h:42
Trk::addNoise
@ addNoise
Definition: MaterialUpdateMode.h:19
Pythia8_RapidityOrderMPI.val
val
Definition: Pythia8_RapidityOrderMPI.py:14
DEBUG
#define DEBUG
Definition: page_access.h:11
MagField::AtlasFieldCache
Local cache for magnetic field (based on MagFieldServices/AtlasFieldSvcTLS.h)
Definition: AtlasFieldCache.h:43
Trk::qOverP
@ qOverP
perigee
Definition: ParamDefs.h:67
python.changerun.pv
pv
Definition: changerun.py:79
MagField::AtlasFieldCache::getField
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,...
Definition: AtlasFieldCache.cxx:42
Trk::TrkV0VertexFitter::m_maxIterations
int m_maxIterations
Definition: TrkV0VertexFitter.h:170
Trk::phi
@ phi
Definition: ParamDefs.h:75
xAOD::TrackParticle_v1
Class describing a TrackParticle.
Definition: TrackParticle_v1.h:43
Trk::TrkV0VertexFitter::initialize
virtual StatusCode initialize() override
Definition: TrkV0VertexFitter.cxx:60
Trk::TrkV0VertexFitter::m_fieldCacheCondObjInputKey
SG::ReadCondHandleKey< AtlasFieldCacheCondObj > m_fieldCacheCondObjInputKey
Definition: TrkV0VertexFitter.h:181
drawFromPickle.sin
sin
Definition: drawFromPickle.py:36
xAOD::Vertex_v1::vxTrackAtVertex
std::vector< Trk::VxTrackAtVertex > & vxTrackAtVertex()
Non-const access to the VxTrackAtVertex vector.
Definition: Vertex_v1.cxx:181
Trk::Surface
Definition: Tracking/TrkDetDescr/TrkSurfaces/TrkSurfaces/Surface.h:79
Trk::removeNoise
@ removeNoise
Definition: MaterialUpdateMode.h:20
makeComparison.deltaR
float deltaR
Definition: makeComparison.py:36
xAOD::Vertex_v1::setCovariancePosition
void setCovariancePosition(const AmgSymMatrix(3)&covariancePosition)
Sets the vertex covariance matrix.
python.AutoConfigFlags.msg
msg
Definition: AutoConfigFlags.py:7
Trk::TrkV0VertexFitter::finalize
virtual StatusCode finalize() override
Definition: TrkV0VertexFitter.cxx:75
TrackParticleContainer.h
fitman.rho
rho
Definition: fitman.py:532
Trk::TrkV0VertexFitter::m_firstMeas
bool m_firstMeas
Definition: TrkV0VertexFitter.h:174
SUSY_SimplifiedModel_PreInclude.masses
dictionary masses
Definition: SUSY_SimplifiedModel_PreInclude.py:7
Generated on Mon Sep 29 2025 21:22:22 for ATLAS Offline Software by   doxygen 1.8.18