CovarianceEngine.cpp

Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file CovarianceEngine.cpp

// This file is part of the Acts project.
//
// Copyright (C) 2019-2020 CERN for the benefit of the Acts project
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "Acts/Propagator/detail/CovarianceEngine.hpp"

namespace Acts {
namespace {
using Jacobian = BoundMatrix;
using Covariance = BoundSymMatrix;
using BoundState = std::tuple<BoundParameters, Jacobian, double>;
using CurvilinearState = std::tuple<CurvilinearParameters, Jacobian, double>;

FreeToBoundMatrix freeToCurvilinearJacobian(const Vector3D& direction) {
  // Optimized trigonometry on the propagation direction
  const double x = direction(0);  // == cos(phi) * sin(theta)
  const double y = direction(1);  // == sin(phi) * sin(theta)
  const double z = direction(2);  // == cos(theta)
  // can be turned into cosine/sine
  const double cosTheta = z;
  const double sinTheta = sqrt(x * x + y * y);
  const double invSinTheta = 1. / sinTheta;
  const double cosPhi = x * invSinTheta;
  const double sinPhi = y * invSinTheta;
  // prepare the jacobian to curvilinear
  FreeToBoundMatrix jacToCurv = FreeToBoundMatrix::Zero();
  if (std::abs(cosTheta) < s_curvilinearProjTolerance) {
    // We normally operate in curvilinear coordinates defined as follows
    jacToCurv(0, 0) = -sinPhi;
    jacToCurv(0, 1) = cosPhi;
    jacToCurv(1, 0) = -cosPhi * cosTheta;
    jacToCurv(1, 1) = -sinPhi * cosTheta;
    jacToCurv(1, 2) = sinTheta;
  } else {
    // Under grazing incidence to z, the above coordinate system definition
    // becomes numerically unstable, and we need to switch to another one
    const double c = sqrt(y * y + z * z);
    const double invC = 1. / c;
    jacToCurv(0, 1) = -z * invC;
    jacToCurv(0, 2) = y * invC;
    jacToCurv(1, 0) = c;
    jacToCurv(1, 1) = -x * y * invC;
    jacToCurv(1, 2) = -x * z * invC;
  }
  // Time parameter
  jacToCurv(5, 3) = 1.;
  // Directional and momentum parameters for curvilinear
  jacToCurv(2, 4) = -sinPhi * invSinTheta;
  jacToCurv(2, 5) = cosPhi * invSinTheta;
  jacToCurv(3, 4) = cosPhi * cosTheta;
  jacToCurv(3, 5) = sinPhi * cosTheta;
  jacToCurv(3, 6) = -sinTheta;
  jacToCurv(4, 7) = 1.;

  return jacToCurv;
}

FreeToBoundMatrix surfaceDerivative(
    std::reference_wrapper<const GeometryContext> geoContext,
    const FreeVector& parameters, BoundToFreeMatrix& jacobianLocalToGlobal,
    const FreeVector& derivatives, const Surface& surface) {
  // Initialize the transport final frame jacobian
  FreeToBoundMatrix jacToLocal = FreeToBoundMatrix::Zero();
  // Initalize the jacobian to local, returns the transposed ref frame
  auto rframeT = surface.initJacobianToLocal(geoContext, jacToLocal,
                                             parameters.segment<3>(eFreePos0),
                                             parameters.segment<3>(eFreeDir0));
  // Calculate the form factors for the derivatives
  const BoundRowVector sVec = surface.derivativeFactors(
      geoContext, parameters.segment<3>(eFreePos0),
      parameters.segment<3>(eFreeDir0), rframeT, jacobianLocalToGlobal);
  jacobianLocalToGlobal -= derivatives * sVec;
  // Return the jacobian to local
  return jacToLocal;
}

const FreeToBoundMatrix surfaceDerivative(
    const Vector3D& direction, BoundToFreeMatrix& jacobianLocalToGlobal,
    const FreeVector& derivatives) {
  // Transport the covariance
  const ActsRowVectorD<3> normVec(direction);
  const BoundRowVector sfactors =
      normVec *
      jacobianLocalToGlobal.template topLeftCorner<3, eBoundParametersSize>();
  jacobianLocalToGlobal -= derivatives * sfactors;
  // Since the jacobian to local needs to calculated for the bound parameters
  // here, it is convenient to do the same here
  return freeToCurvilinearJacobian(direction);
}

void reinitializeJacobians(
    std::reference_wrapper<const GeometryContext> geoContext,
    FreeMatrix& transportJacobian, FreeVector& derivatives,
    BoundToFreeMatrix& jacobianLocalToGlobal, const FreeVector& parameters,
    const Surface& surface) {
  using VectorHelpers::phi;
  using VectorHelpers::theta;

  // Reset the jacobians
  transportJacobian = FreeMatrix::Identity();
  derivatives = FreeVector::Zero();
  jacobianLocalToGlobal = BoundToFreeMatrix::Zero();

  // Reset the jacobian from local to global
  Vector2D loc{0., 0.};
  const Vector3D position = parameters.segment<3>(eFreePos0);
  const Vector3D direction = parameters.segment<3>(eFreeDir0);
  surface.globalToLocal(geoContext, position, direction, loc);
  BoundVector pars;
  pars << loc[eLOC_0], loc[eLOC_1], phi(direction), theta(direction),
      parameters[eFreeQOverP], parameters[eFreeTime];
  surface.initJacobianToGlobal(geoContext, jacobianLocalToGlobal, position,
                               direction, pars);
}

void reinitializeJacobians(FreeMatrix& transportJacobian,
                           FreeVector& derivatives,
                           BoundToFreeMatrix& jacobianLocalToGlobal,
                           const Vector3D& direction) {
  // Reset the jacobians
  transportJacobian = FreeMatrix::Identity();
  derivatives = FreeVector::Zero();
  jacobianLocalToGlobal = BoundToFreeMatrix::Zero();

  // Optimized trigonometry on the propagation direction
  const double x = direction(0);  // == cos(phi) * sin(theta)
  const double y = direction(1);  // == sin(phi) * sin(theta)
  const double z = direction(2);  // == cos(theta)
  // can be turned into cosine/sine
  const double cosTheta = z;
  const double sinTheta = sqrt(x * x + y * y);
  const double invSinTheta = 1. / sinTheta;
  const double cosPhi = x * invSinTheta;
  const double sinPhi = y * invSinTheta;

  jacobianLocalToGlobal(0, eLOC_0) = -sinPhi;
  jacobianLocalToGlobal(0, eLOC_1) = -cosPhi * cosTheta;
  jacobianLocalToGlobal(1, eLOC_0) = cosPhi;
  jacobianLocalToGlobal(1, eLOC_1) = -sinPhi * cosTheta;
  jacobianLocalToGlobal(2, eLOC_1) = sinTheta;
  jacobianLocalToGlobal(3, eT) = 1;
  jacobianLocalToGlobal(4, ePHI) = -sinTheta * sinPhi;
  jacobianLocalToGlobal(4, eTHETA) = cosTheta * cosPhi;
  jacobianLocalToGlobal(5, ePHI) = sinTheta * cosPhi;
  jacobianLocalToGlobal(5, eTHETA) = cosTheta * sinPhi;
  jacobianLocalToGlobal(6, eTHETA) = -sinTheta;
  jacobianLocalToGlobal(7, eQOP) = 1;
}
}  // namespace

namespace detail {

BoundState boundState(std::reference_wrapper<const GeometryContext> geoContext,
                      Covariance& covarianceMatrix, Jacobian& jacobian,
                      FreeMatrix& transportJacobian, FreeVector& derivatives,
                      BoundToFreeMatrix& jacobianLocalToGlobal,
                      const FreeVector& parameters, bool covTransport,
                      double accumulatedPath, const Surface& surface) {
  // Covariance transport
  std::optional<BoundSymMatrix> cov = std::nullopt;
  if (covTransport) {
    covarianceTransport(geoContext, covarianceMatrix, jacobian,
                        transportJacobian, derivatives, jacobianLocalToGlobal,
                        parameters, surface);
    cov = covarianceMatrix;
  }
  // Create the bound parameters
  const Vector3D& position = parameters.segment<3>(eFreePos0);
  const Vector3D momentum =
      std::abs(1. / parameters[eFreeQOverP]) * parameters.segment<3>(eFreeDir0);
  const double charge = std::copysign(1., parameters[eFreeQOverP]);
  const double time = parameters[eFreeTime];
  BoundParameters boundParameters(geoContext, cov, position, momentum, charge,
                                  time, surface.getSharedPtr());
  // Create the bound state
  return std::make_tuple(std::move(boundParameters), jacobian, accumulatedPath);
  ;
}

CurvilinearState curvilinearState(Covariance& covarianceMatrix,
                                  Jacobian& jacobian,
                                  FreeMatrix& transportJacobian,
                                  FreeVector& derivatives,
                                  BoundToFreeMatrix& jacobianLocalToGlobal,
                                  const FreeVector& parameters,
                                  bool covTransport, double accumulatedPath) {
  const Vector3D& direction = parameters.segment<3>(eFreeDir0);

  // Covariance transport
  std::optional<BoundSymMatrix> cov = std::nullopt;
  if (covTransport) {
    covarianceTransport(covarianceMatrix, jacobian, transportJacobian,
                        derivatives, jacobianLocalToGlobal, direction);
    cov = covarianceMatrix;
  }
  // Create the curvilinear parameters
  const Vector3D& position = parameters.segment<3>(eFreePos0);
  const Vector3D momentum = std::abs(1. / parameters[eFreeQOverP]) * direction;
  const double charge = std::copysign(1., parameters[eFreeQOverP]);
  const double time = parameters[eFreeTime];
  CurvilinearParameters curvilinearParameters(cov, position, momentum, charge,
                                              time);
  // Create the curvilinear state
  return std::make_tuple(std::move(curvilinearParameters), jacobian,
                         accumulatedPath);
}

void covarianceTransport(Covariance& covarianceMatrix, Jacobian& jacobian,
                         FreeMatrix& transportJacobian, FreeVector& derivatives,
                         BoundToFreeMatrix& jacobianLocalToGlobal,
                         const Vector3D& direction) {
  // Build the full jacobian
  jacobianLocalToGlobal = transportJacobian * jacobianLocalToGlobal;
  const FreeToBoundMatrix jacToLocal =
      surfaceDerivative(direction, jacobianLocalToGlobal, derivatives);
  const Jacobian jacFull = jacToLocal * jacobianLocalToGlobal;

  // Apply the actual covariance transport
  covarianceMatrix = jacFull * covarianceMatrix * jacFull.transpose();

  // Reinitialize jacobian components
  reinitializeJacobians(transportJacobian, derivatives, jacobianLocalToGlobal,
                        direction);

  // Store The global and bound jacobian (duplication for the moment)
  jacobian = jacFull;
}

void covarianceTransport(
    std::reference_wrapper<const GeometryContext> geoContext,
    Covariance& covarianceMatrix, Jacobian& jacobian,
    FreeMatrix& transportJacobian, FreeVector& derivatives,
    BoundToFreeMatrix& jacobianLocalToGlobal, const FreeVector& parameters,
    const Surface& surface) {
  // Build the full jacobian
  jacobianLocalToGlobal = transportJacobian * jacobianLocalToGlobal;
  const FreeToBoundMatrix jacToLocal = surfaceDerivative(
      geoContext, parameters, jacobianLocalToGlobal, derivatives, surface);
  const Jacobian jacFull = jacToLocal * jacobianLocalToGlobal;

  // Apply the actual covariance transport
  covarianceMatrix = jacFull * covarianceMatrix * jacFull.transpose();

  // Reinitialize jacobian components
  reinitializeJacobians(geoContext, transportJacobian, derivatives,
                        jacobianLocalToGlobal, parameters, surface);

  // Store The global and bound jacobian (duplication for the moment)
  jacobian = jacFull;
}
}  // namespace detail
}  // namespace Acts