Propagator/detail/SympyCovarianceEngine.cpp

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/Propagator/detail/SympyCovarianceEngine.hpp"
0010
0011 #include "Acts/Definitions/TrackParametrization.hpp"
0012 #include "Acts/Propagator/detail/JacobianEngine.hpp"
0013 #include "Acts/Propagator/detail/SympyJacobianEngine.hpp"
0014
0015 #include "codegen/sympy_cov_math.hpp"
0016
0017 namespace Acts::detail {
0018
0019 /// Some type defs
0020 using Jacobian = BoundMatrix;
0021 using BoundState = std::tuple<BoundTrackParameters, Jacobian, double>;
0022
0023 Result<BoundState> sympy::boundState(
0024     const GeometryContext& geoContext, const Surface& surface,
0025     BoundSquareMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0026     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0027     BoundToFreeMatrix& boundToFreeJacobian,
0028     const std::optional<FreeMatrix>& additionalFreeCovariance,
0029     FreeVector& freeParameters, const ParticleHypothesis& particleHypothesis,
0030     bool covTransport, double accumulatedPath,
0031     const FreeToBoundCorrection& freeToBoundCorrection) {
0032   // Create the bound parameters
0033   Result<BoundVector> bv =
0034       transformFreeToBoundParameters(freeParameters, surface, geoContext);
0035   if (!bv.ok()) {
0036     return bv.error();
0037   }
0038
0039   // Covariance transport
0040   std::optional<BoundSquareMatrix> cov = std::nullopt;
0041   if (covTransport) {
0042     // Calculate the jacobian and transport the covarianceMatrix to final local.
0043     // Then reinitialize the transportJacobian, derivatives and the
0044     // boundToFreeJacobian
0045     transportCovarianceToBound(
0046         geoContext, surface, boundCovariance, fullTransportJacobian,
0047         freeTransportJacobian, freeToPathDerivatives, boundToFreeJacobian,
0048         additionalFreeCovariance, freeParameters, freeToBoundCorrection);
0049     cov = boundCovariance;
0050   }
0051
0052   // Create the bound state
0053   return std::make_tuple(
0054       BoundTrackParameters(surface.getSharedPtr(), *bv, std::move(cov),
0055                            particleHypothesis),
0056       fullTransportJacobian, accumulatedPath);
0057 }
0058
0059 BoundState sympy::curvilinearState(
0060     BoundSquareMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0061     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0062     BoundToFreeMatrix& boundToFreeJacobian,
0063     const std::optional<FreeMatrix>& additionalFreeCovariance,
0064     const FreeVector& freeParameters,
0065     const ParticleHypothesis& particleHypothesis, bool covTransport,
0066     double accumulatedPath) {
0067   const Vector3& direction = freeParameters.segment<3>(eFreeDir0);
0068
0069   // Covariance transport
0070   std::optional<BoundSquareMatrix> cov = std::nullopt;
0071   if (covTransport) {
0072     // Calculate the jacobian and transport the covarianceMatrix to final local.
0073     // Then reinitialize the transportJacobian, derivatives and the
0074     // boundToFreeJacobian
0075     transportCovarianceToCurvilinear(boundCovariance, fullTransportJacobian,
0076                                      freeTransportJacobian,
0077                                      freeToPathDerivatives, boundToFreeJacobian,
0078                                      additionalFreeCovariance, direction);
0079     cov = boundCovariance;
0080   }
0081
0082   // Create the curvilinear parameters
0083   Vector4 pos4 = Vector4::Zero();
0084   pos4[ePos0] = freeParameters[eFreePos0];
0085   pos4[ePos1] = freeParameters[eFreePos1];
0086   pos4[ePos2] = freeParameters[eFreePos2];
0087   pos4[eTime] = freeParameters[eFreeTime];
0088   BoundTrackParameters curvilinearParams =
0089       BoundTrackParameters::createCurvilinear(
0090           pos4, direction, freeParameters[eFreeQOverP], std::move(cov),
0091           particleHypothesis);
0092   // Create the curvilinear state
0093   return {std::move(curvilinearParams), fullTransportJacobian, accumulatedPath};
0094 }
0095
0096 void sympy::transportCovarianceToBound(
0097     const GeometryContext& geoContext, const Surface& surface,
0098     BoundSquareMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0099     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0100     BoundToFreeMatrix& boundToFreeJacobian,
0101     const std::optional<FreeMatrix>& additionalFreeCovariance,
0102     FreeVector& freeParameters,
0103     const FreeToBoundCorrection& freeToBoundCorrection) {
0104   FreeToBoundMatrix freeToBoundJacobian;
0105
0106   // Calculate the full jacobian from local parameters at the start surface to
0107   // current bound parameters
0108   sympy::boundToBoundTransportJacobian(
0109       geoContext, surface, freeParameters, boundToFreeJacobian,
0110       freeTransportJacobian, freeToBoundJacobian, freeToPathDerivatives,
0111       fullTransportJacobian);
0112
0113   bool correction = false;
0114   if (freeToBoundCorrection) {
0115     BoundToFreeMatrix startBoundToFinalFreeJacobian =
0116         freeTransportJacobian * boundToFreeJacobian;
0117     FreeSquareMatrix freeCovariance = startBoundToFinalFreeJacobian *
0118                                       boundCovariance *
0119                                       startBoundToFinalFreeJacobian.transpose();
0120
0121     auto transformer =
0122         detail::CorrectedFreeToBoundTransformer(freeToBoundCorrection);
0123     auto correctedRes =
0124         transformer(freeParameters, freeCovariance, surface, geoContext);
0125
0126     if (correctedRes.has_value()) {
0127       auto correctedValue = correctedRes.value();
0128       BoundVector boundParams = std::get<BoundVector>(correctedValue);
0129       // 1. Update the free parameters with the corrected bound parameters
0130       freeParameters =
0131           transformBoundToFreeParameters(surface, geoContext, boundParams);
0132
0133       // 2. Update the bound covariance
0134       boundCovariance = std::get<BoundSquareMatrix>(correctedValue);
0135
0136       correction = true;
0137     }
0138   }
0139
0140   if (!correction) {
0141     // Apply the actual covariance transport to get covariance of the current
0142     // bound parameters
0143     BoundMatrix newBoundCovariance;
0144     transportCovarianceToBoundImpl(boundCovariance.data(),
0145                                    fullTransportJacobian.data(),
0146                                    newBoundCovariance.data());
0147     boundCovariance = newBoundCovariance;
0148   }
0149
0150   if (additionalFreeCovariance) {
0151     boundCovariance += freeToBoundJacobian * (*additionalFreeCovariance) *
0152                        freeToBoundJacobian.transpose();
0153   }
0154
0155   // Reinitialize jacobian components:
0156   // ->The transportJacobian is reinitialized to Identity
0157   // ->The derivatives is reinitialized to Zero
0158   // ->The boundToFreeJacobian is initialized to that at the current surface
0159   reinitializeJacobians(geoContext, surface, freeTransportJacobian,
0160                         freeToPathDerivatives, boundToFreeJacobian,
0161                         freeParameters);
0162 }
0163
0164 void sympy::transportCovarianceToCurvilinear(
0165     BoundSquareMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0166     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0167     BoundToFreeMatrix& boundToFreeJacobian,
0168     const std::optional<FreeMatrix>& additionalFreeCovariance,
0169     const Vector3& direction) {
0170   FreeToBoundMatrix freeToBoundJacobian;
0171
0172   // Calculate the full jacobian from local parameters at the start surface to
0173   // current curvilinear parameters
0174   sympy::boundToCurvilinearTransportJacobian(
0175       direction, boundToFreeJacobian, freeTransportJacobian,
0176       freeToBoundJacobian, freeToPathDerivatives, fullTransportJacobian);
0177
0178   // Apply the actual covariance transport to get covariance of the current
0179   // curvilinear parameters
0180   BoundMatrix newBoundCovariance;
0181   transportCovarianceToBoundImpl(boundCovariance.data(),
0182                                  fullTransportJacobian.data(),
0183                                  newBoundCovariance.data());
0184   boundCovariance = newBoundCovariance;
0185
0186   if (additionalFreeCovariance) {
0187     boundCovariance += freeToBoundJacobian * (*additionalFreeCovariance) *
0188                        freeToBoundJacobian.transpose();
0189   }
0190
0191   // Reinitialize jacobian components:
0192   // ->The free transportJacobian is reinitialized to Identity
0193   // ->The path derivatives is reinitialized to Zero
0194   // ->The boundToFreeJacobian is reinitialized to that at the current
0195   // curvilinear surface
0196   reinitializeJacobians(freeTransportJacobian, freeToPathDerivatives,
0197                         boundToFreeJacobian, direction);
0198 }
0199
0200 }  // namespace Acts::detail