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