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
0017 namespace Acts {
0018
0019 void detail::boundToBoundTransportJacobian(
0020     const GeometryContext& geoContext, const Surface& surface,
0021     const FreeVector& freeParameters,
0022     const BoundToFreeMatrix& boundToFreeJacobian,
0023     const FreeMatrix& freeTransportJacobian,
0024     FreeToBoundMatrix& freeToBoundJacobian,
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   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   FreeToBoundMatrix freeToBoundJacobian;
0054   detail::boundToBoundTransportJacobian(
0055       geoContext, surface, freeParameters, boundToFreeJacobian,
0056       freeTransportJacobian, freeToBoundJacobian, freeToPathDerivatives,
0057       result);
0058   return result;
0059 }
0060
0061 void detail::boundToCurvilinearTransportJacobian(
0062     const Vector3& direction, const BoundToFreeMatrix& boundToFreeJacobian,
0063     const FreeMatrix& freeTransportJacobian,
0064     FreeToBoundMatrix& freeToBoundJacobian,
0065     const FreeVector& freeToPathDerivatives,
0066     BoundMatrix& fullTransportJacobian) {
0067   // Calculate the jacobian from global to local at the curvilinear surface
0068   freeToBoundJacobian = CurvilinearSurface(direction).freeToBoundJacobian();
0069
0070   // Update the jacobian to include the derivative of the path length at the
0071   // curvilinear surface w.r.t. the free parameters
0072   freeToBoundJacobian.topLeftCorner<6, 3>() +=
0073       (freeToBoundJacobian * freeToPathDerivatives) *
0074       (-1.0 * direction).transpose();
0075
0076   // Calculate the full jocobian from the local parameters at the start surface
0077   // to curvilinear parameters
0078   // @note jac(locA->locB) = jac(gloB->locB)*(1+
0079   // pathCorrectionFactor(gloB))*jacTransport(gloA->gloB) *jac(locA->gloA)
0080   fullTransportJacobian =
0081       freeToBoundJacobian * freeTransportJacobian * boundToFreeJacobian;
0082 }
0083
0084 BoundToFreeMatrix detail::boundToFreeTransportJacobian(
0085     const BoundToFreeMatrix& boundToFreeJacobian,
0086     const FreeMatrix& freeTransportJacobian) {
0087   // Calculate the full jacobian, in this case simple a product of
0088   // jacobian(transport in free) * jacobian(bound to free)
0089   return freeTransportJacobian * boundToFreeJacobian;
0090 }
0091
0092 void detail::freeToBoundTransportJacobian(
0093     const GeometryContext& geoContext, const Surface& surface,
0094     const FreeVector& freeParameters, const FreeMatrix& freeTransportJacobian,
0095     const FreeVector& freeToPathDerivatives,
0096     FreeToBoundMatrix& fullTransportJacobian) {
0097   const Vector3 position = freeParameters.segment<3>(eFreePos0);
0098   const Vector3 direction = freeParameters.segment<3>(eFreeDir0);
0099   // Calculate the jacobian from free to bound at the final surface
0100   FreeToBoundMatrix freeToBoundJacobian =
0101       surface.freeToBoundJacobian(geoContext, position, direction);
0102   FreeToPathMatrix sVec =
0103       surface.freeToPathDerivative(geoContext, position, direction);
0104   // Return the jacobian to local
0105   fullTransportJacobian = freeToBoundJacobian * (freeTransportJacobian +
0106                                                  freeToPathDerivatives * sVec *
0107                                                      freeTransportJacobian);
0108 }
0109
0110 FreeToBoundMatrix detail::freeToCurvilinearTransportJacobian(
0111     const Vector3& direction, const FreeMatrix& freeTransportJacobian,
0112     const FreeVector& freeToPathDerivatives) {
0113   auto sfactors = direction.transpose() *
0114                   freeTransportJacobian.template topLeftCorner<3, 8>();
0115
0116   // Since the jacobian to local needs to calculated for the bound parameters
0117   // here, it is convenient to do the same here
0118   return CurvilinearSurface(direction).freeToBoundJacobian() *
0119          (freeTransportJacobian - freeToPathDerivatives * sfactors);
0120 }
0121
0122 Result<void> detail::reinitializeJacobians(
0123     const GeometryContext& geoContext, const Surface& surface,
0124     FreeMatrix& freeTransportJacobian, FreeVector& freeToPathDerivatives,
0125     BoundToFreeMatrix& boundToFreeJacobian, const FreeVector& freeParameters) {
0126   // Reset the jacobians
0127   freeTransportJacobian = FreeMatrix::Identity();
0128   freeToPathDerivatives = FreeVector::Zero();
0129
0130   // Get the local position
0131   const Vector3 position = freeParameters.segment<3>(eFreePos0);
0132   const Vector3 direction = freeParameters.segment<3>(eFreeDir0);
0133   auto lpResult = surface.globalToLocal(geoContext, position, direction);
0134   if (!lpResult.ok()) {
0135     return lpResult.error();
0136   }
0137   // Reset the jacobian from local to global
0138   boundToFreeJacobian =
0139       surface.boundToFreeJacobian(geoContext, position, direction);
0140   return Result<void>::success();
0141 }
0142
0143 void detail::reinitializeJacobians(FreeMatrix& freeTransportJacobian,
0144                                    FreeVector& freeToPathDerivatives,
0145                                    BoundToFreeMatrix& boundToFreeJacobian,
0146                                    const Vector3& direction) {
0147   // Reset the jacobians
0148   freeTransportJacobian = FreeMatrix::Identity();
0149   freeToPathDerivatives = FreeVector::Zero();
0150   boundToFreeJacobian = CurvilinearSurface(direction).boundToFreeJacobian();
0151 }
0152
0153 }  // namespace Acts