Propagator/detail/JacobianEngine.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/JacobianEngine.hpp"
0010
0011 #include "Acts/Definitions/Algebra.hpp"
0012 #include "Acts/Definitions/TrackParametrization.hpp"
0013 #include "Acts/Geometry/GeometryContext.hpp"
0014 #include "Acts/Surfaces/CurvilinearSurface.hpp"
0015 #include "Acts/Surfaces/Surface.hpp"
0016 #include "Acts/Utilities/AlgebraHelpers.hpp"
0017
0018 namespace Acts {
0019
0020 void detail::boundToBoundTransportJacobian(
0021     const GeometryContext& geoContext, const Surface& surface,
0022     const FreeVector& freeParameters,
0023     const BoundToFreeMatrix& boundToFreeJacobian,
0024     const FreeMatrix& freeTransportJacobian,
0025     const FreeVector& freeToPathDerivatives,
0026     BoundMatrix& fullTransportJacobian) {
0027   const Vector3 position = freeParameters.segment<3>(eFreePos0);
0028   const Vector3 direction = freeParameters.segment<3>(eFreeDir0);
0029   // Calculate the derivative of path length at the final surface or the
0030   // point-of-closest approach w.r.t. free parameters
0031   const FreeToPathMatrix freeToPath =
0032       surface.freeToPathDerivative(geoContext, position, direction);
0033   // Calculate the jacobian from free to bound at the final surface
0034   FreeToBoundMatrix freeToBoundJacobian =
0035       surface.freeToBoundJacobian(geoContext, position, direction);
0036   // https://acts.readthedocs.io/en/latest/white_papers/correction-for-transport-jacobian.html
0037   // Calculate the full jacobian from the local/bound parameters at the start
0038   // surface to local/bound parameters at the final surface
0039   // @note jac(locA->locB) = jac(gloB->locB)*(1+
0040   // pathCorrectionFactor(gloB))*jacTransport(gloA->gloB) *jac(locA->gloA)
0041   fullTransportJacobian =
0042       freeToBoundJacobian *
0043       (FreeMatrix::Identity() + freeToPathDerivatives * freeToPath) *
0044       freeTransportJacobian * boundToFreeJacobian;
0045 }
0046
0047 BoundMatrix detail::boundToBoundTransportJacobian(
0048     const GeometryContext& geoContext, const FreeVector& freeParameters,
0049     const BoundToFreeMatrix& boundToFreeJacobian,
0050     const FreeMatrix& freeTransportJacobian,
0051     const FreeVector& freeToPathDerivatives, const Surface& surface) {
0052   BoundMatrix result;
0053   detail::boundToBoundTransportJacobian(
0054       geoContext, surface, freeParameters, boundToFreeJacobian,
0055       freeTransportJacobian, freeToPathDerivatives, result);
0056   return result;
0057 }
0058
0059 void detail::boundToCurvilinearTransportJacobian(
0060     const Vector3& direction, const BoundToFreeMatrix& boundToFreeJacobian,
0061     const FreeMatrix& freeTransportJacobian,
0062     const FreeVector& freeToPathDerivatives,
0063     BoundMatrix& fullTransportJacobian) {
0064   // Calculate the jacobian from global to local at the curvilinear surface
0065   FreeToBoundMatrix freeToBoundJacobian =
0066       CurvilinearSurface(direction).freeToBoundJacobian();
0067
0068   // Update the jacobian to include the derivative of the path length at the
0069   // curvilinear surface w.r.t. the free parameters
0070   freeToBoundJacobian.topLeftCorner<6, 3>() +=
0071       (freeToBoundJacobian * freeToPathDerivatives) *
0072       (-1.0 * direction).transpose();
0073
0074   // Calculate the full jocobian from the local parameters at the start surface
0075   // to curvilinear parameters
0076   // @note jac(locA->locB) = jac(gloB->locB)*(1+
0077   // pathCorrectionFactor(gloB))*jacTransport(gloA->gloB) *jac(locA->gloA)
0078   fullTransportJacobian =
0079       freeToBoundJacobian * freeTransportJacobian * boundToFreeJacobian;
0080 }
0081
0082 BoundToFreeMatrix detail::boundToFreeTransportJacobian(
0083     const BoundToFreeMatrix& boundToFreeJacobian,
0084     const FreeMatrix& freeTransportJacobian) {
0085   // Calculate the full jacobian, in this case simple a product of
0086   // jacobian(transport in free) * jacobian(bound to free)
0087   return freeTransportJacobian * boundToFreeJacobian;
0088 }
0089
0090 void detail::freeToBoundTransportJacobian(
0091     const GeometryContext& geoContext, const Surface& surface,
0092     const FreeVector& freeParameters, const FreeMatrix& freeTransportJacobian,
0093     const FreeVector& freeToPathDerivatives,
0094     FreeToBoundMatrix& fullTransportJacobian) {
0095   const Vector3 position = freeParameters.segment<3>(eFreePos0);
0096   const Vector3 direction = freeParameters.segment<3>(eFreeDir0);
0097   // Calculate the jacobian from free to bound at the final surface
0098   FreeToBoundMatrix freeToBoundJacobian =
0099       surface.freeToBoundJacobian(geoContext, position, direction);
0100   FreeToPathMatrix sVec =
0101       surface.freeToPathDerivative(geoContext, position, direction);
0102   // Return the jacobian to local
0103   fullTransportJacobian = freeToBoundJacobian * (freeTransportJacobian +
0104                                                  freeToPathDerivatives * sVec *
0105                                                      freeTransportJacobian);
0106 }
0107
0108 FreeToBoundMatrix detail::freeToCurvilinearTransportJacobian(
0109     const Vector3& direction, const FreeMatrix& freeTransportJacobian,
0110     const FreeVector& freeToPathDerivatives) {
0111   auto sfactors = direction.transpose() *
0112                   freeTransportJacobian.template topLeftCorner<3, 8>();
0113
0114   // Since the jacobian to local needs to calculated for the bound parameters
0115   // here, it is convenient to do the same here
0116   return CurvilinearSurface(direction).freeToBoundJacobian() *
0117          (freeTransportJacobian - freeToPathDerivatives * sfactors);
0118 }
0119
0120 Result<void> detail::reinitializeJacobians(
0121     const GeometryContext& geoContext, const Surface& surface,
0122     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0123     BoundToFreeMatrix& boundToFreeJacobian, const FreeVector& freeParameters) {
0124   // Reset the jacobians
0125   freeTransportJacobian = FreeMatrix::Identity();
0126   freeToPathDerivatives = FreeVector::Zero();
0127
0128   // Get the local position
0129   const Vector3 position = freeParameters.segment<3>(eFreePos0);
0130   const Vector3 direction = freeParameters.segment<3>(eFreeDir0);
0131   auto lpResult = surface.globalToLocal(geoContext, position, direction);
0132   if (!lpResult.ok()) {
0133     return lpResult.error();
0134   }
0135   // Reset the jacobian from local to global
0136   boundToFreeJacobian =
0137       surface.boundToFreeJacobian(geoContext, position, direction);
0138   return Result<void>::success();
0139 }
0140
0141 void detail::reinitializeJacobians(FreeMatrix& freeTransportJacobian,
0142                                    FreeVector& freeToPathDerivatives,
0143                                    BoundToFreeMatrix& boundToFreeJacobian,
0144                                    const Vector3& direction) {
0145   // Reset the jacobians
0146   freeTransportJacobian = FreeMatrix::Identity();
0147   freeToPathDerivatives = FreeVector::Zero();
0148   boundToFreeJacobian = CurvilinearSurface(direction).boundToFreeJacobian();
0149 }
0150
0151 }  // namespace Acts