ATLAS Offline Software
Loading...
Searching...
No Matches
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
22within anonymous namespace. They contain temporary calculations of matrices
23and vectors resulting from the vertex calculation. */
24namespace
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
37namespace 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
M_PI
#define M_PI
Definition ActiveFraction.h:11
deltaR
Scalar deltaR(const MatrixBase< Derived > &vec) const
Definition AmgMatrixBasePlugin.h:105
endmsg
#define endmsg
Definition AnalysisConfig_Ntuple.cxx:63
ATH_CHECK
#define ATH_CHECK
Evaluate an expression and check for errors.
Definition AthCheckMacros.h:40
ATH_MSG_FATAL
#define ATH_MSG_FATAL(x)
Definition AthMsgStreamMacros.h:34
ATH_MSG_VERBOSE
#define ATH_MSG_VERBOSE(x)
Definition AthMsgStreamMacros.h:28
ATH_MSG_DEBUG
#define ATH_MSG_DEBUG(x)
Definition AthMsgStreamMacros.h:29
charge
double charge(const T &p)
Definition AtlasPID.h:997
AmgSymMatrix
#define AmgSymMatrix(dim)
Definition EventPrimitives.h:50
AmgVector
#define AmgVector(rows)
Definition EventPrimitives.h:55
Vertex
boost::graph_traits< boost::adjacency_list< boost::vecS, boost::vecS, boost::bidirectionalS > >::vertex_descriptor Vertex
Definition FPGATrackSimGNNRoadMakerTool.h:35
AtlasFieldCacheCondObj::getInitializedCache
void getInitializedCache(MagField::AtlasFieldCache &cache) const
get B field cache for evaluation as a function of 2-d or 3-d position.
Definition AtlasFieldCacheCondObj.h:32
MagField::AtlasFieldCache
Local cache for magnetic field (based on MagFieldServices/AtlasFieldSvcTLS.h)
Definition AtlasFieldCache.h:43
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
SG::AuxElement::makePrivateStore
void makePrivateStore()
Create a new (empty) private store for this object.
Definition AuxElement.cxx:192
Trk::CylinderSurface
Class for a CylinderSurface in the ATLAS detector.
Definition CylinderSurface.h:55
Trk::ParametersBase::position
const Amg::Vector3D & position() const
Access method for the position.
Trk::PerigeeSurface
Class describing the Line to which the Perigee refers to.
Definition PerigeeSurface.h:43
Trk::Surface
Abstract Base Class for tracking surfaces.
Definition Tracking/TrkDetDescr/TrkSurfaces/TrkSurfaces/Surface.h:79
Trk::TrkV0VertexFitter::initialize
virtual StatusCode initialize() override
Definition TrkV0VertexFitter.cxx:60
Trk::TrkV0VertexFitter::TrkV0VertexFitter
TrkV0VertexFitter(const std::string &t, const std::string &n, const IInterface *p)
Definition TrkV0VertexFitter.cxx:39
Trk::TrkV0VertexFitter::m_fieldCacheCondObjInputKey
SG::ReadCondHandleKey< AtlasFieldCacheCondObj > m_fieldCacheCondObjInputKey
Definition TrkV0VertexFitter.h:181
Trk::TrkV0VertexFitter::m_extrapolator
ToolHandle< Trk::IExtrapolator > m_extrapolator
Data members to store the results.
Definition TrkV0VertexFitter.h:179
Trk::TrkV0VertexFitter::finalize
virtual StatusCode finalize() override
Definition TrkV0VertexFitter.cxx:75
Trk::TrkV0VertexFitter::~TrkV0VertexFitter
virtual ~TrkV0VertexFitter()
standard destructor
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
Trk::Vertex
This class is a simplest representation of a vertex candidate.
Definition Tracking/TrkEvent/VxVertex/VxVertex/Vertex.h:26
xAOD::Vertex_v1::setCovariancePosition
void setCovariancePosition(const AmgSymMatrix(3)&covariancePosition)
Sets the vertex covariance matrix.
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
xAOD::Vertex_v1::setVertexType
void setVertexType(VxType::VertexType vType)
Set the type of the vertex.
xAOD::Vertex_v1::setCovariance
void setCovariance(const std::vector< float > &value)
Sets the covariance matrix as a simple vector of values.
xAOD::Vertex_v1::setPosition
void setPosition(const Amg::Vector3D &position)
Sets the 3-position.
xAOD::Vertex_v1::vxTrackAtVertex
std::vector< Trk::VxTrackAtVertex > & vxTrackAtVertex()
Non-const access to the VxTrackAtVertex vector.
Definition Vertex_v1.cxx:181
xAOD::Vertex_v1::setFitQuality
void setFitQuality(float chiSquared, float numberDoF)
Set the 'Fit Quality' information.
Definition Vertex_v1.cxx:150
xAOD::Vertex_v1::position
const Amg::Vector3D & position() const
Returns the 3-pos.
chi2
double chi2(TH1 *h0, TH1 *h1)
Definition comparitor.cxx:525
Amg::MatrixX
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
Definition EventPrimitives.h:27
Amg::Transform3D
Eigen::Affine3d Transform3D
Definition GeoPrimitives.h:46
Amg::Vector3D
Eigen::Matrix< double, 3, 1 > Vector3D
Definition GeoPrimitives.h:47
Amg::VectorX
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorX
Dynamic Vector - dynamic allocation.
Definition EventPrimitives.h:30
Trk::alongMomentum
@ alongMomentum
Definition PropDirection.h:20
Trk::anyDirection
@ anyDirection
Definition PropDirection.h:22
Trk::Perigee
ParametersT< TrackParametersDim, Charged, PerigeeSurface > Perigee
Definition Tracking/TrkEvent/TrkParameters/TrkParameters/TrackParameters.h:33
Trk::CurvilinearParameters
CurvilinearParametersT< TrackParametersDim, Charged, PlaneSurface > CurvilinearParameters
Definition Tracking/TrkEvent/TrkParameters/TrkParameters/TrackParameters.h:29
Trk::theta
@ theta
Definition ParamDefs.h:66
Trk::qOverP
@ qOverP
perigee
Definition ParamDefs.h:67
Trk::phi
@ phi
Definition ParamDefs.h:75
Trk::d0
@ d0
Definition ParamDefs.h:63
Trk::z0
@ z0
Definition ParamDefs.h:64
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::TrackParameters
ParametersBase< TrackParametersDim, Charged > TrackParameters
Definition Tracking/TrkEvent/TrkParameters/TrkParameters/TrackParameters.h:27
xAOD::VxType::V0Vtx
@ V0Vtx
Vertex from V0 decay.
Definition TrackingPrimitives.h:576
xAOD::TrackParticle
TrackParticle_v1 TrackParticle
Reference the current persistent version:
Definition Event/xAOD/xAODTracking/xAODTracking/TrackParticle.h:13
xAOD::Vertex
Vertex_v1 Vertex
Define the latest version of the vertex class.
Definition Event/xAOD/xAODTracking/xAODTracking/Vertex.h:16
xAOD::FirstMeasurement
@ FirstMeasurement
Parameter defined at the position of the 1st measurement.
Definition TrackingPrimitives.h:214
msg
MsgStream & msg
Definition testRead.cxx:32
Generated on for ATLAS Offline Software by doxygen 1.14.0