Back to home page

EIC code displayed by LXR

 
 

    


File indexing completed on 2025-07-06 07:52:16

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008 
0009 #include "Acts/Vertexing/FullBilloirVertexFitter.hpp"
0010 
0011 #include "Acts/Definitions/TrackParametrization.hpp"
0012 #include "Acts/MagneticField/ConstantBField.hpp"
0013 #include "Acts/Surfaces/PerigeeSurface.hpp"
0014 #include "Acts/Utilities/detail/periodic.hpp"
0015 #include "Acts/Vertexing/TrackAtVertex.hpp"
0016 #include "Acts/Vertexing/VertexingError.hpp"
0017 
0018 namespace {
0019 
0020 /// @struct BilloirTrack
0021 ///
0022 /// @brief Struct to cache track-specific matrix operations in Billoir fitter
0023 struct BilloirTrack {
0024   explicit BilloirTrack(const Acts::InputTrack& params)
0025       : originalTrack(params) {}
0026 
0027   BilloirTrack(const BilloirTrack& arg) = default;
0028 
0029   Acts::InputTrack originalTrack;
0030   double chi2 = 0;
0031 
0032   // We drop the summation index i from Ref. (1) for better readability
0033   Acts::ActsMatrix<Acts::eBoundSize, Acts::eBoundSize> W;  // Wi weight matrix
0034   Acts::ActsMatrix<Acts::eBoundSize, 4> D;  // Di (position Jacobian)
0035   Acts::ActsMatrix<Acts::eBoundSize, 3> E;  // Ei (momentum Jacobian)
0036   Acts::ActsSquareMatrix<3> C;              //  = sum{Ei^T Wi * Ei}
0037   Acts::ActsMatrix<4, 3> B;                 //  = Di^T * Wi * Ei
0038   Acts::ActsSquareMatrix<3> Cinv;           //  = (Ei^T * Wi * Ei)^-1
0039   Acts::Vector3 U;                          //  = Ei^T * Wi * dqi
0040   Acts::ActsMatrix<4, 3> BCinv;             //  = Bi * Ci^-1
0041   Acts::BoundVector deltaQ;
0042 };
0043 
0044 /// @struct BilloirVertex
0045 ///
0046 /// @brief Struct to cache vertex-specific matrix operations in Billoir fitter
0047 struct BilloirVertex {
0048   // A  = sum{Di^T * Wi * Di}
0049   Acts::SquareMatrix4 A = Acts::SquareMatrix4::Zero();
0050   // T  = sum{Di^T * Wi * dqi}
0051   Acts::Vector4 T = Acts::Vector4::Zero();
0052   // sumBCinvBt = sum{Bi * Ci^-1 * Bi^T}
0053   Acts::SquareMatrix4 sumBCinvBt = Acts::SquareMatrix4::Zero();
0054   // sumBCinvU = sum{B * Ci^-1 * Ui}
0055   Acts::Vector4 sumBCinvU = Acts::Vector4::Zero();
0056 };
0057 
0058 }  // namespace
0059 
0060 Acts::Result<Acts::Vertex> Acts::FullBilloirVertexFitter::fit(
0061     const std::vector<InputTrack>& paramVector,
0062     const VertexingOptions& vertexingOptions,
0063     MagneticFieldProvider::Cache& fieldCache) const {
0064   unsigned int nTracks = paramVector.size();
0065   double chi2 = std::numeric_limits<double>::max();
0066 
0067   if (nTracks == 0) {
0068     return Vertex(Vector3(0., 0., 0.));
0069   }
0070 
0071   // Set number of degrees of freedom following Eq. 8.28 from Ref. (2):
0072   //
0073   // ndf = sum_i=1^nTracks rank(Wi^-1) - 3 * (nTracks + 1),
0074   //
0075   // where W_i denotes the weight matrix of the i-th track.
0076   // Assuming rank(W_i) = #(Perigee params) = 6 for all tracks, we have
0077   //
0078   // ndf = 3 * nTracks - 3.
0079   int ndf = 3 * nTracks - 3;
0080   // TODO: this seems strange - can we even do a vertex fit if we only have one
0081   // track?
0082   if (nTracks < 2) {
0083     ndf = 1;
0084   }
0085 
0086   // Since we add a term to the chi2 when adding a vertex constraint (see Ref.
0087   // (1)), the number of degrees of freedom increases
0088   if (vertexingOptions.useConstraintInFit) {
0089     ndf += 3;
0090   }
0091 
0092   std::vector<BilloirTrack> billoirTracks;
0093   std::vector<Vector3> trackMomenta;
0094   // Initial guess of the 4D vertex position
0095   Vector4 linPoint = vertexingOptions.constraint.fullPosition();
0096   Vertex fittedVertex;
0097 
0098   for (int nIter = 0; nIter < m_cfg.maxIterations; ++nIter) {
0099     billoirTracks.clear();
0100     double newChi2 = 0;
0101     BilloirVertex billoirVertex;
0102 
0103     Vector3 linPointPos = VectorHelpers::position(linPoint);
0104     // Make Perigee surface at linPointPos, transverse plane of Perigee
0105     // corresponds the global x-y plane
0106     const std::shared_ptr<PerigeeSurface> perigeeSurface =
0107         Surface::makeShared<PerigeeSurface>(linPointPos);
0108 
0109     // iterate over all tracks
0110     for (std::size_t iTrack = 0; iTrack < nTracks; ++iTrack) {
0111       const InputTrack& trackContainer = paramVector[iTrack];
0112 
0113       const auto& trackParams = m_cfg.extractParameters(trackContainer);
0114 
0115       auto result =
0116           m_cfg.trackLinearizer(trackParams, linPoint[3], *perigeeSurface,
0117                                 vertexingOptions.geoContext,
0118                                 vertexingOptions.magFieldContext, fieldCache);
0119       if (!result.ok()) {
0120         return result.error();
0121       }
0122 
0123       const auto& linTrack = *result;
0124       const auto& parametersAtPCA = linTrack.parametersAtPCA;
0125       double d0 = parametersAtPCA[BoundIndices::eBoundLoc0];
0126       double z0 = parametersAtPCA[BoundIndices::eBoundLoc1];
0127       double phi = parametersAtPCA[BoundIndices::eBoundPhi];
0128       double theta = parametersAtPCA[BoundIndices::eBoundTheta];
0129       double qOverP = parametersAtPCA[BoundIndices::eBoundQOverP];
0130       double t0 = parametersAtPCA[BoundIndices::eBoundTime];
0131 
0132       // Take the track momenta at the PCA as an initial estimate of the track
0133       // momenta at the vertex
0134       if (nIter == 0) {
0135         trackMomenta.push_back(Vector3(phi, theta, qOverP));
0136       }
0137 
0138       // Calculate F(V_0,p_0), i.e., the track parameters estimated from the
0139       // vertex position and the track momenta. fD0 = fZ0 = 0 because the track
0140       // originates at the vertex in the Billoir model.
0141       double fPhi = trackMomenta[iTrack][0];
0142       double fTheta = trackMomenta[iTrack][1];
0143       double fQOvP = trackMomenta[iTrack][2];
0144       double fTime = linPoint[FreeIndices::eFreeTime];
0145       BilloirTrack billoirTrack(trackContainer);
0146 
0147       billoirTrack.deltaQ << d0, z0, phi - fPhi, theta - fTheta, qOverP - fQOvP,
0148           t0 - fTime;
0149 
0150       // position jacobian (D matrix)
0151       ActsMatrix<eBoundSize, 4> D = linTrack.positionJacobian;
0152 
0153       // momentum jacobian (E matrix)
0154       ActsMatrix<eBoundSize, 3> E = linTrack.momentumJacobian;
0155 
0156       // cache some matrix multiplications
0157       BoundSquareMatrix W = linTrack.weightAtPCA;
0158       ActsMatrix<4, eBoundSize> DtW = D.transpose() * W;
0159       ActsMatrix<3, eBoundSize> EtW = E.transpose() * W;
0160 
0161       // compute track quantities for Billoir fit
0162       billoirTrack.D = D;
0163       billoirTrack.E = E;
0164       billoirTrack.W = W;
0165       billoirTrack.C = EtW * E;
0166       billoirTrack.B = DtW * E;                        // Di^T * Wi * Ei
0167       billoirTrack.U = EtW * billoirTrack.deltaQ;      // Ei^T * Wi * dqi
0168       billoirTrack.Cinv = (billoirTrack.C).inverse();  // (Ei^T * Wi * Ei)^-1
0169       billoirTrack.BCinv =
0170           billoirTrack.B * billoirTrack.Cinv;  // BCinv = Bi * Ci^-1
0171 
0172       // compute vertex quantities for Billoir fit
0173       billoirVertex.T += DtW * billoirTrack.deltaQ;  // sum{Di^T * Wi * dqi}
0174       billoirVertex.A += DtW * D;                    // sum{Di^T * Wi * Di}
0175       billoirVertex.sumBCinvU +=
0176           billoirTrack.BCinv * billoirTrack.U;  // sum{Bi * Ci^-1 * Ui}
0177       billoirVertex.sumBCinvBt +=
0178           billoirTrack.BCinv *
0179           billoirTrack.B.transpose();  // sum{Bi * Ci^-1 * Bi^T}
0180 
0181       billoirTracks.push_back(billoirTrack);
0182     }  // end loop tracks
0183 
0184     // (Matrix-valued) factor contributing to the vertex estimate update (i.e.,
0185     // to deltaV).
0186     // deltaVFac = T-sum{Bi*Ci^-1*Ui}
0187     Vector4 deltaVFac = billoirVertex.T - billoirVertex.sumBCinvU;
0188 
0189     // Inverse of the covariance matrix of the 4D vertex position (note that
0190     // Cov(V) = Cov(deltaV)), see Ref. (1).
0191     // invCovV = A-sum{Bi*Ci^-1*Bi^T}
0192     SquareMatrix4 invCovV = billoirVertex.A - billoirVertex.sumBCinvBt;
0193     if (vertexingOptions.useConstraintInFit) {
0194       // Position of vertex constraint in Billoir frame (i.e., in coordinate
0195       // system with origin at linPoint). This will be 0 for first iteration but
0196       // != 0 from second on since our first guess for the vertex position is
0197       // the vertex constraint position.
0198       Vector4 posInBilloirFrame =
0199           vertexingOptions.constraint.fullPosition() - linPoint;
0200 
0201       // For vertex constraint: T -> T + Cb^-1 (b - V0) where Cb is the
0202       // covariance matrix of the constraint, b is the constraint position, and
0203       // V0 is the vertex estimate (see Ref. (1)).
0204       deltaVFac += vertexingOptions.constraint.fullCovariance().inverse() *
0205                    posInBilloirFrame;
0206       // For vertex constraint: A -> A + Cb^-1
0207       invCovV += vertexingOptions.constraint.fullCovariance().inverse();
0208     }
0209 
0210     // Covariance matrix of the 4D vertex position, see Ref. (3)
0211     SquareMatrix4 covV = invCovV.inverse();
0212     // Update of the vertex position
0213     Vector4 deltaV = covV * deltaVFac;
0214     //--------------------------------------------------------------------------------------
0215     // start momentum related calculations
0216 
0217     std::vector<std::optional<BoundSquareMatrix>> covDeltaP(nTracks);
0218 
0219     // Update track momenta and calculate the covariance of the track parameters
0220     // after the fit (TODO: parameters -> momenta).
0221     for (std::size_t iTrack = 0; iTrack < billoirTracks.size(); ++iTrack) {
0222       auto& billoirTrack = billoirTracks[iTrack];
0223 
0224       Vector3 deltaP = (billoirTrack.Cinv) *
0225                        (billoirTrack.U - billoirTrack.B.transpose() * deltaV);
0226 
0227       // update track momenta
0228       trackMomenta[iTrack] += deltaP;
0229 
0230       // correct for 2PI / PI periodicity
0231       const auto correctedPhiTheta = detail::normalizePhiTheta(
0232           trackMomenta[iTrack][0], trackMomenta[iTrack][1]);
0233       trackMomenta[iTrack][0] = correctedPhiTheta.first;
0234       trackMomenta[iTrack][1] = correctedPhiTheta.second;
0235 
0236       // Calculate chi2 of the track...
0237       Acts::BoundVector diffQ = billoirTrack.deltaQ - billoirTrack.D * deltaV -
0238                                 billoirTrack.E * deltaP;
0239       billoirTrack.chi2 = diffQ.transpose().dot(billoirTrack.W * diffQ);
0240       // ... and add it to the total chi2 value
0241       newChi2 += billoirTrack.chi2;
0242 
0243       // calculate the 6x6 cross-covariance matrix
0244       // TODO: replace fittedParams with fittedMomentum since d0 and z0 are
0245       // ill-defined. At the moment, only the submatrix of momentum covariances
0246       // is correct.
0247 
0248       // coordinate transformation matrix, i.e.,
0249       // d(d0,z0,phi,theta,qOverP,t)/d(x,y,z,t,phi,theta,qOverP)
0250       // TODO: This is not correct since the (x, y, z, t) in the derivatives in
0251       // the D matrix correspond to the global track position at the PCA rather
0252       // than the 4D vertex position.
0253       ActsMatrix<eBoundSize, 7> transMat;
0254       transMat.setZero();
0255       transMat.block<2, 2>(0, 0) = billoirTrack.D.template block<2, 2>(0, 0);
0256       transMat(1, 2) = 1.;
0257       transMat(2, 4) = 1.;
0258       transMat(3, 5) = 1.;
0259       transMat(4, 6) = 1.;
0260       transMat(5, 3) = 1.;
0261 
0262       // cov(V,P)
0263       ActsMatrix<4, 3> covVP = -covV * billoirTrack.BCinv;
0264 
0265       // cov(P,P), 3x3 matrix, see Ref. (3)
0266       ActsSquareMatrix<3> covP =
0267           billoirTrack.Cinv +
0268           billoirTrack.BCinv.transpose() * covV * billoirTrack.BCinv;
0269 
0270       ActsSquareMatrix<7> cov;
0271       cov.setZero();
0272       cov.block<4, 4>(0, 0) = covV;
0273       cov.block<4, 3>(0, 4) = covVP;
0274       cov.block<3, 4>(4, 0) = covVP.transpose();
0275       cov.block<3, 3>(4, 4) = covP;
0276 
0277       covDeltaP[iTrack] = transMat * cov * transMat.transpose();
0278     }
0279 
0280     // assign new linearization point (= new vertex position in global frame)
0281     linPoint += deltaV;
0282 
0283     // Add an additional term to chi2 if there is a vertex constraint (see Ref.
0284     // (1))
0285     if (vertexingOptions.useConstraintInFit) {
0286       // Position of vertex constraint in Billoir frame (i.e., in coordinate
0287       // system with origin at linPoint).
0288       Vector4 posInBilloirFrame =
0289           vertexingOptions.constraint.fullPosition() - linPoint;
0290 
0291       newChi2 +=
0292           (posInBilloirFrame.transpose())
0293               .dot(vertexingOptions.constraint.fullCovariance().inverse() *
0294                    posInBilloirFrame);
0295     }
0296 
0297     if (!std::isnormal(newChi2)) {
0298       ACTS_ERROR("Encountered non-normal chi2 value during the fit.");
0299       return VertexingError::NumericFailure;
0300     }
0301 
0302     if (newChi2 < chi2) {
0303       chi2 = newChi2;
0304 
0305       fittedVertex.setFullPosition(linPoint);
0306       fittedVertex.setFullCovariance(covV);
0307       fittedVertex.setFitQuality(chi2, ndf);
0308 
0309       std::vector<TrackAtVertex> tracksAtVertex;
0310 
0311       std::shared_ptr<PerigeeSurface> perigee =
0312           Surface::makeShared<PerigeeSurface>(
0313               VectorHelpers::position(linPoint));
0314 
0315       for (std::size_t iTrack = 0; iTrack < billoirTracks.size(); ++iTrack) {
0316         const auto& billoirTrack = billoirTracks[iTrack];
0317 
0318         // TODO: replace refittedParams with refittedMomenta since d0 and z0
0319         // after the vertex fit are ill-defined (FullBilloir only outputs the
0320         // updated track momenta)
0321 
0322         // new refitted trackparameters
0323         BoundVector paramVec = BoundVector::Zero();
0324         paramVec[eBoundPhi] = trackMomenta[iTrack](0);
0325         paramVec[eBoundTheta] = trackMomenta[iTrack](1);
0326         paramVec[eBoundQOverP] = trackMomenta[iTrack](2);
0327         paramVec[eBoundTime] = linPoint[FreeIndices::eFreeTime];
0328         BoundTrackParameters refittedParams(
0329             perigee, paramVec, covDeltaP[iTrack],
0330             m_cfg.extractParameters(billoirTrack.originalTrack)
0331                 .particleHypothesis());
0332         TrackAtVertex trackAtVertex(billoirTrack.chi2, refittedParams,
0333                                     billoirTrack.originalTrack);
0334         tracksAtVertex.push_back(std::move(trackAtVertex));
0335       }
0336 
0337       fittedVertex.setTracksAtVertex(std::move(tracksAtVertex));
0338     }
0339   }  // end loop iterations
0340 
0341   return fittedVertex;
0342 }