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