Propagator/detail/CovarianceEngine.cpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2019-2020 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 http://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::make_tuple(std::move(curvilinearParams), fullTransportJacobian,
0101                          accumulatedPath);
0102 }
0103
0104 void detail::transportCovarianceToBound(
0105     const GeometryContext& geoContext, const Surface& surface,
0106     BoundSquareMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0107     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0108     BoundToFreeMatrix& boundToFreeJacobian, FreeVector& freeParameters,
0109     const FreeToBoundCorrection& freeToBoundCorrection) {
0110   // Calculate the full jacobian from local parameters at the start surface to
0111   // current bound parameters
0112   boundToBoundTransportJacobian(geoContext, surface, freeParameters,
0113                                 boundToFreeJacobian, freeTransportJacobian,
0114                                 freeToPathDerivatives, fullTransportJacobian);
0115
0116   bool correction = false;
0117   if (freeToBoundCorrection) {
0118     BoundToFreeMatrix startBoundToFinalFreeJacobian =
0119         freeTransportJacobian * boundToFreeJacobian;
0120     FreeSquareMatrix freeCovariance = startBoundToFinalFreeJacobian *
0121                                       boundCovariance *
0122                                       startBoundToFinalFreeJacobian.transpose();
0123
0124     auto transformer =
0125         detail::CorrectedFreeToBoundTransformer(freeToBoundCorrection);
0126     auto correctedRes =
0127         transformer(freeParameters, freeCovariance, surface, geoContext);
0128
0129     if (correctedRes.has_value()) {
0130       auto correctedValue = correctedRes.value();
0131       BoundVector boundParams = std::get<BoundVector>(correctedValue);
0132       // 1. Update the free parameters with the corrected bound parameters
0133       freeParameters =
0134           transformBoundToFreeParameters(surface, geoContext, boundParams);
0135
0136       // 2. Update the bound covariance
0137       boundCovariance = std::get<BoundSquareMatrix>(correctedValue);
0138
0139       correction = true;
0140     }
0141   }
0142
0143   if (!correction) {
0144     // Apply the actual covariance transport to get covariance of the current
0145     // bound parameters
0146     boundCovariance = fullTransportJacobian * boundCovariance *
0147                       fullTransportJacobian.transpose();
0148   }
0149
0150   // Reinitialize jacobian components:
0151   // ->The transportJacobian is reinitialized to Identity
0152   // ->The derivatives is reinitialized to Zero
0153   // ->The boundToFreeJacobian is initialized to that at the current surface
0154   reinitializeJacobians(geoContext, surface, freeTransportJacobian,
0155                         freeToPathDerivatives, boundToFreeJacobian,
0156                         freeParameters);
0157 }
0158
0159 void detail::transportCovarianceToCurvilinear(
0160     BoundSquareMatrix& boundCovariance, BoundMatrix& fullTransportJacobian,
0161     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0162     BoundToFreeMatrix& boundToFreeJacobian, const Vector3& direction) {
0163   // Calculate the full jacobian from local parameters at the start surface to
0164   // current curvilinear parameters
0165   boundToCurvilinearTransportJacobian(
0166       direction, boundToFreeJacobian, freeTransportJacobian,
0167       freeToPathDerivatives, fullTransportJacobian);
0168
0169   // Apply the actual covariance transport to get covariance of the current
0170   // curvilinear parameters
0171   boundCovariance = fullTransportJacobian * boundCovariance *
0172                     fullTransportJacobian.transpose();
0173
0174   // Reinitialize jacobian components:
0175   // ->The free transportJacobian is reinitialized to Identity
0176   // ->The path derivatives is reinitialized to Zero
0177   // ->The boundToFreeJacobian is reinitialized to that at the current
0178   // curvilinear surface
0179   reinitializeJacobians(freeTransportJacobian, freeToPathDerivatives,
0180                         boundToFreeJacobian, direction);
0181 }
0182
0183 Acts::Result<Acts::BoundTrackParameters> detail::boundToBoundConversion(
0184     const GeometryContext& gctx, const BoundTrackParameters& boundParameters,
0185     const Surface& targetSurface, const Vector3& bField) {
0186   const auto& sourceSurface = boundParameters.referenceSurface();
0187
0188   Acts::FreeVector freePars = Acts::transformBoundToFreeParameters(
0189       sourceSurface, gctx, boundParameters.parameters());
0190
0191   auto res =
0192       Acts::transformFreeToBoundParameters(freePars, targetSurface, gctx);
0193
0194   if (!res.ok()) {
0195     return res.error();
0196   }
0197   Acts::BoundVector parOut = *res;
0198
0199   std::optional<Acts::BoundMatrix> covOut = std::nullopt;
0200
0201   if (boundParameters.covariance().has_value()) {
0202     Acts::BoundToFreeMatrix boundToFreeJacobian =
0203         sourceSurface.boundToFreeJacobian(gctx, freePars.segment<3>(eFreePos0),
0204                                           freePars.segment<3>(eFreeDir0));
0205
0206     Acts::FreeMatrix freeTransportJacobian = FreeMatrix::Identity();
0207
0208     FreeVector freeToPathDerivatives = FreeVector::Zero();
0209     freeToPathDerivatives.head<3>() = freePars.segment<3>(eFreeDir0);
0210
0211     freeToPathDerivatives.segment<3>(eFreeDir0) =
0212         bField.cross(freePars.segment<3>(eFreeDir0));
0213
0214     BoundMatrix boundToBoundJac;
0215     detail::boundToBoundTransportJacobian(
0216         gctx, targetSurface, freePars, boundToFreeJacobian,
0217         freeTransportJacobian, freeToPathDerivatives, boundToBoundJac);
0218
0219     covOut = boundToBoundJac * (*boundParameters.covariance()) *
0220              boundToBoundJac.transpose();
0221   }
0222
0223   return Acts::BoundTrackParameters{targetSurface.getSharedPtr(), parOut,
0224                                     covOut,
0225                                     boundParameters.particleHypothesis()};
0226 }
0227
0228 }  // namespace Acts