Propagator/detail/CovarianceEngine.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/CovarianceEngine.hpp"
0010
0011 #include "Acts/Definitions/Common.hpp"
0012 #include "Acts/Definitions/TrackParametrization.hpp"
0013 #include "Acts/EventData/BoundTrackParameters.hpp"
0014 #include "Acts/EventData/TransformationHelpers.hpp"
0015 #include "Acts/EventData/detail/CorrectedTransformationFreeToBound.hpp"
0016 #include "Acts/Propagator/detail/JacobianEngine.hpp"
0017 #include "Acts/Utilities/Result.hpp"
0018
0019 #include <optional>
0020 #include <system_error>
0021 #include <utility>
0022
0023 namespace Acts {
0024
0025 /// Some type defs
0026 using Jacobian = BoundMatrix;
0027 using BoundState = std::tuple<BoundTrackParameters, Jacobian, double>;
0028
0029 Result<BoundState> detail::boundState(
0030     const GeometryContext& geoContext, const Surface& surface,
0031     BoundMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0032     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0033     BoundToFreeMatrix& boundToFreeJacobian,
0034     const std::optional<FreeMatrix>& additionalFreeCovariance,
0035     FreeVector& freeParameters, const ParticleHypothesis& particleHypothesis,
0036     bool covTransport, double accumulatedPath,
0037     const FreeToBoundCorrection& freeToBoundCorrection) {
0038   // Create the bound parameters
0039   Result<BoundVector> bv =
0040       transformFreeToBoundParameters(freeParameters, surface, geoContext);
0041   if (!bv.ok()) {
0042     return bv.error();
0043   }
0044
0045   // Covariance transport
0046   std::optional<BoundMatrix> cov = std::nullopt;
0047   if (covTransport) {
0048     // Calculate the jacobian and transport the covarianceMatrix to final local.
0049     // Then reinitialize the transportJacobian, derivatives and the
0050     // boundToFreeJacobian
0051     transportCovarianceToBound(
0052         geoContext, surface, boundCovariance, fullTransportJacobian,
0053         freeTransportJacobian, freeToPathDerivatives, boundToFreeJacobian,
0054         additionalFreeCovariance, freeParameters, freeToBoundCorrection);
0055     cov = boundCovariance;
0056   }
0057
0058   // Create the bound state
0059   return std::make_tuple(
0060       BoundTrackParameters(surface.getSharedPtr(), *bv, std::move(cov),
0061                            particleHypothesis),
0062       fullTransportJacobian, accumulatedPath);
0063 }
0064
0065 BoundState detail::curvilinearState(
0066     BoundMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0067     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0068     BoundToFreeMatrix& boundToFreeJacobian,
0069     const std::optional<FreeMatrix>& additionalFreeCovariance,
0070     const FreeVector& freeParameters,
0071     const ParticleHypothesis& particleHypothesis, bool covTransport,
0072     double accumulatedPath) {
0073   const Vector3& direction = freeParameters.segment<3>(eFreeDir0);
0074
0075   // Covariance transport
0076   std::optional<BoundMatrix> cov = std::nullopt;
0077   if (covTransport) {
0078     // Calculate the jacobian and transport the covarianceMatrix to final local.
0079     // Then reinitialize the transportJacobian, derivatives and the
0080     // boundToFreeJacobian
0081     transportCovarianceToCurvilinear(boundCovariance, fullTransportJacobian,
0082                                      freeTransportJacobian,
0083                                      freeToPathDerivatives, boundToFreeJacobian,
0084                                      additionalFreeCovariance, direction);
0085     cov = boundCovariance;
0086   }
0087
0088   // Create the curvilinear parameters
0089   Vector4 pos4 = Vector4::Zero();
0090   pos4[ePos0] = freeParameters[eFreePos0];
0091   pos4[ePos1] = freeParameters[eFreePos1];
0092   pos4[ePos2] = freeParameters[eFreePos2];
0093   pos4[eTime] = freeParameters[eFreeTime];
0094   BoundTrackParameters curvilinearParams =
0095       BoundTrackParameters::createCurvilinear(
0096           pos4, direction, freeParameters[eFreeQOverP], std::move(cov),
0097           particleHypothesis);
0098   // Create the curvilinear state
0099   return {std::move(curvilinearParams), fullTransportJacobian, accumulatedPath};
0100 }
0101
0102 void detail::transportCovarianceToBound(
0103     const GeometryContext& geoContext, const Surface& surface,
0104     BoundMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0105     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0106     BoundToFreeMatrix& boundToFreeJacobian,
0107     const std::optional<FreeMatrix>& additionalFreeCovariance,
0108     FreeVector& freeParameters,
0109     const FreeToBoundCorrection& freeToBoundCorrection) {
0110   FreeToBoundMatrix freeToBoundJacobian;
0111
0112   // Calculate the full jacobian from local parameters at the start surface to
0113   // current bound parameters
0114   boundToBoundTransportJacobian(geoContext, surface, freeParameters,
0115                                 boundToFreeJacobian, freeTransportJacobian,
0116                                 freeToBoundJacobian, freeToPathDerivatives,
0117                                 fullTransportJacobian);
0118
0119   bool correction = false;
0120   if (freeToBoundCorrection) {
0121     BoundToFreeMatrix startBoundToFinalFreeJacobian =
0122         freeTransportJacobian * boundToFreeJacobian;
0123     FreeMatrix freeCovariance = startBoundToFinalFreeJacobian *
0124                                 boundCovariance *
0125                                 startBoundToFinalFreeJacobian.transpose();
0126
0127     auto transformer =
0128         detail::CorrectedFreeToBoundTransformer(freeToBoundCorrection);
0129     auto correctedRes =
0130         transformer(freeParameters, freeCovariance, surface, geoContext);
0131
0132     if (correctedRes.has_value()) {
0133       auto correctedValue = correctedRes.value();
0134       BoundVector boundParams = std::get<BoundVector>(correctedValue);
0135       // 1. Update the free parameters with the corrected bound parameters
0136       freeParameters =
0137           transformBoundToFreeParameters(surface, geoContext, boundParams);
0138
0139       // 2. Update the bound covariance
0140       boundCovariance = std::get<BoundMatrix>(correctedValue);
0141
0142       correction = true;
0143     }
0144   }
0145
0146   if (!correction) {
0147     // Apply the actual covariance transport to get covariance of the current
0148     // bound parameters
0149     boundCovariance = fullTransportJacobian * boundCovariance *
0150                       fullTransportJacobian.transpose();
0151   }
0152
0153   if (additionalFreeCovariance) {
0154     boundCovariance += freeToBoundJacobian * (*additionalFreeCovariance) *
0155                        freeToBoundJacobian.transpose();
0156   }
0157
0158   // Reinitialize jacobian components:
0159   // ->The transportJacobian is reinitialized to Identity
0160   // ->The derivatives is reinitialized to Zero
0161   // ->The boundToFreeJacobian is initialized to that at the current surface
0162   reinitializeJacobians(geoContext, surface, freeTransportJacobian,
0163                         freeToPathDerivatives, boundToFreeJacobian,
0164                         freeParameters);
0165 }
0166
0167 void detail::transportCovarianceToCurvilinear(
0168     BoundMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0169     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0170     BoundToFreeMatrix& boundToFreeJacobian,
0171     const std::optional<FreeMatrix>& additionalFreeCovariance,
0172     const Vector3& direction) {
0173   FreeToBoundMatrix freeToBoundJacobian;
0174
0175   // Calculate the full jacobian from local parameters at the start surface to
0176   // current curvilinear parameters
0177   boundToCurvilinearTransportJacobian(
0178       direction, boundToFreeJacobian, freeTransportJacobian,
0179       freeToBoundJacobian, freeToPathDerivatives, fullTransportJacobian);
0180
0181   // Apply the actual covariance transport to get covariance of the current
0182   // curvilinear parameters
0183   boundCovariance = fullTransportJacobian * boundCovariance *
0184                     fullTransportJacobian.transpose();
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 Result<BoundTrackParameters> detail::boundToBoundConversion(
0201     const GeometryContext& gctx, const BoundTrackParameters& boundParameters,
0202     const Surface& targetSurface, const Vector3& bField) {
0203   const auto& sourceSurface = boundParameters.referenceSurface();
0204
0205   FreeVector freePars = transformBoundToFreeParameters(
0206       sourceSurface, gctx, boundParameters.parameters());
0207
0208   auto res = transformFreeToBoundParameters(freePars, targetSurface, gctx);
0209
0210   if (!res.ok()) {
0211     return res.error();
0212   }
0213   BoundVector parOut = *res;
0214
0215   std::optional<BoundMatrix> covOut = std::nullopt;
0216
0217   if (boundParameters.covariance().has_value()) {
0218     BoundToFreeMatrix boundToFreeJacobian = sourceSurface.boundToFreeJacobian(
0219         gctx, freePars.segment<3>(eFreePos0), freePars.segment<3>(eFreeDir0));
0220
0221     FreeMatrix freeTransportJacobian = FreeMatrix::Identity();
0222
0223     FreeVector freeToPathDerivatives = FreeVector::Zero();
0224     freeToPathDerivatives.head<3>() = freePars.segment<3>(eFreeDir0);
0225
0226     freeToPathDerivatives.segment<3>(eFreeDir0) =
0227         bField.cross(freePars.segment<3>(eFreeDir0));
0228
0229     BoundMatrix boundToBoundJac;
0230     FreeToBoundMatrix freeToBoundJacobian;
0231     detail::boundToBoundTransportJacobian(
0232         gctx, targetSurface, freePars, boundToFreeJacobian,
0233         freeTransportJacobian, freeToBoundJacobian, freeToPathDerivatives,
0234         boundToBoundJac);
0235
0236     covOut = boundToBoundJac * (*boundParameters.covariance()) *
0237              boundToBoundJac.transpose();
0238   }
0239
0240   return BoundTrackParameters{targetSurface.getSharedPtr(), parOut, covOut,
0241                               boundParameters.particleHypothesis()};
0242 }
0243
0244 }  // namespace Acts