src/EventData/CorrectedTransformationFreeToBound.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/EventData/detail/CorrectedTransformationFreeToBound.hpp"
0010
0011 #include "Acts/Definitions/TrackParametrization.hpp"
0012 #include "Acts/EventData/TransformationHelpers.hpp"
0013 #include "Acts/Surfaces/RegularSurface.hpp"
0014 #include "Acts/Surfaces/Surface.hpp"
0015 #include "Acts/Utilities/Intersection.hpp"
0016 #include "Acts/Utilities/Result.hpp"
0017 #include "Acts/Utilities/ThrowAssert.hpp"
0018
0019 #include <algorithm>
0020 #include <cmath>
0021 #include <cstddef>
0022 #include <memory>
0023 #include <ostream>
0024 #include <type_traits>
0025 #include <utility>
0026 #include <vector>
0027
0028 Acts::FreeToBoundCorrection::FreeToBoundCorrection(bool apply_, double alpha_,
0029                                                    double beta_)
0030     : apply(apply_), alpha(alpha_), beta(beta_) {}
0031
0032 Acts::FreeToBoundCorrection::FreeToBoundCorrection(bool apply_)
0033     : apply(apply_) {}
0034
0035 Acts::FreeToBoundCorrection::operator bool() const {
0036   return apply;
0037 }
0038
0039 Acts::detail::CorrectedFreeToBoundTransformer::CorrectedFreeToBoundTransformer(
0040     double alpha, double beta, double cosIncidentAngleMinCutoff,
0041     double cosIncidentAngleMaxCutoff)
0042     : m_alpha(alpha),
0043       m_beta(beta),
0044       m_cosIncidentAngleMinCutoff(cosIncidentAngleMinCutoff),
0045       m_cosIncidentAngleMaxCutoff(cosIncidentAngleMaxCutoff) {}
0046
0047 Acts::detail::CorrectedFreeToBoundTransformer::CorrectedFreeToBoundTransformer(
0048     const FreeToBoundCorrection& freeToBoundCorrection) {
0049   m_alpha = freeToBoundCorrection.alpha;
0050   m_beta = freeToBoundCorrection.beta;
0051   m_cosIncidentAngleMinCutoff = freeToBoundCorrection.cosIncidentAngleMinCutoff;
0052   m_cosIncidentAngleMaxCutoff = freeToBoundCorrection.cosIncidentAngleMaxCutoff;
0053 }
0054
0055 std::optional<std::tuple<Acts::BoundVector, Acts::BoundSquareMatrix>>
0056 Acts::detail::CorrectedFreeToBoundTransformer::operator()(
0057     const Acts::FreeVector& freeParams,
0058     const Acts::FreeSquareMatrix& freeCovariance, const Acts::Surface& surface,
0059     const Acts::GeometryContext& geoContext, Direction navDir,
0060     const Logger& logger) const {
0061   // Get the incidence angle
0062   Vector3 dir = freeParams.segment<3>(eFreeDir0);
0063   Vector3 normal =
0064       surface.normal(geoContext, freeParams.segment<3>(eFreePos0), dir);
0065   double absCosIncidenceAng = std::abs(dir.dot(normal));
0066   // No correction if the incidentAngle is small enough (not necessary ) or too
0067   // large (correction could be invalid). Fall back to nominal free to bound
0068   // transformation
0069   if (absCosIncidenceAng < m_cosIncidentAngleMinCutoff ||
0070       absCosIncidenceAng > m_cosIncidentAngleMaxCutoff) {
0071     ACTS_VERBOSE("Incident angle: " << std::acos(absCosIncidenceAng)
0072                                     << " is out of range for correction");
0073     return std::nullopt;
0074   }
0075
0076   // The number of sigma points
0077   std::size_t sampleSize = 2 * eFreeSize + 1;
0078   // The sampled free parameters, the weight for measurement W_m and weight for
0079   // covariance, W_c
0080   std::vector<std::tuple<FreeVector, double, double>> sampledFreeParams;
0081   sampledFreeParams.reserve(sampleSize);
0082
0083   // Initialize the covariance sqrt root matrix
0084   FreeSquareMatrix covSqrt = FreeSquareMatrix::Zero();
0085   // SVD decomposition: freeCovariance = U*S*U^T here
0086   Eigen::JacobiSVD<FreeSquareMatrix> svd(
0087       freeCovariance, Eigen::ComputeFullU | Eigen::ComputeFullV);
0088   auto S = svd.singularValues();
0089   FreeMatrix U = svd.matrixU();
0090   // Get the sqrt root matrix of S
0091   FreeMatrix D = FreeMatrix::Zero();
0092   for (unsigned i = 0; i < eFreeSize; ++i) {
0093     if (S(i) > 0) {
0094       D(i, i) = std::sqrt(S(i));
0095     }
0096   }
0097   // Get the covariance sqrt root matrix
0098   covSqrt = U * D;
0099
0100   // Define kappa = alpha*alpha*N
0101   double kappa = m_alpha * m_alpha * static_cast<double>(eFreeSize);
0102   // lambda = alpha*alpha*N - N
0103   double lambda = kappa - static_cast<double>(eFreeSize);
0104   // gamma = sqrt(labmda + N)
0105   double gamma = std::sqrt(kappa);
0106
0107   // Sample the free parameters
0108   // 1. the nominal parameter
0109   sampledFreeParams.push_back(
0110       {freeParams, lambda / kappa,
0111        lambda / kappa + (1.0 - m_alpha * m_alpha + m_beta)});
0112   // 2. the shifted parameters
0113   for (unsigned i = 0; i < eFreeSize; ++i) {
0114     sampledFreeParams.push_back(
0115         {freeParams + covSqrt.col(i) * gamma, 0.5 / kappa, 0.5 / kappa});
0116     sampledFreeParams.push_back(
0117         {freeParams - covSqrt.col(i) * gamma, 0.5 / kappa, 0.5 / kappa});
0118   }
0119
0120   // Initialize the mean of the bound parameters
0121   BoundVector bpMean = BoundVector::Zero();
0122   // Initialize the bound covariance
0123   BoundSquareMatrix bv = BoundSquareMatrix::Zero();
0124
0125   // The transformed bound parameters and weight for each sampled free
0126   // parameters
0127   std::vector<std::pair<BoundVector, double>> transformedBoundParams;
0128
0129   // 1. The nominal one
0130   // The sampled free parameters, the weight for measurement W_m and weight for
0131   // covariance, W_c
0132   const auto& [paramsNom, mweightNom, cweightNom] = sampledFreeParams[0];
0133   // Transform the free to bound
0134   auto nominalRes =
0135       transformFreeToBoundParameters(paramsNom, surface, geoContext);
0136   // Not successful, fall back to nominal free to bound transformation
0137   if (!nominalRes.ok()) {
0138     ACTS_WARNING(
0139         "Free to bound transformation for nominal free parameters failed.");
0140     return std::nullopt;
0141   }
0142   auto nominalBound = nominalRes.value();
0143   transformedBoundParams.push_back({nominalBound, cweightNom});
0144   bpMean = bpMean + mweightNom * nominalBound;
0145
0146   // 2. Loop over the rest sample points of the free parameters to get the
0147   // corrected bound parameters
0148   for (unsigned i = 1; i < sampledFreeParams.size(); ++i) {
0149     const auto& [params, mweight, cweight] = sampledFreeParams[i];
0150     FreeVector correctedFreeParams = params;
0151
0152     // Reintersect to get the corrected free params without boundary check
0153     SurfaceIntersection intersection =
0154         surface
0155             .intersect(geoContext, params.segment<3>(eFreePos0),
0156                        navDir * params.segment<3>(eFreeDir0),
0157                        BoundaryTolerance::Infinite())
0158             .closest();
0159     correctedFreeParams.segment<3>(eFreePos0) = intersection.position();
0160
0161     // Transform the free to bound
0162     auto result = transformFreeToBoundParameters(correctedFreeParams, surface,
0163                                                  geoContext);
0164     // Not successful, fall back to nominal free to bound transformation
0165     if (!result.ok()) {
0166       ACTS_WARNING(
0167           "Free to bound transformation for sampled free parameters: \n"
0168           << correctedFreeParams << " failed.");
0169       return std::nullopt;
0170     }
0171
0172     auto bp = result.value();
0173     transformedBoundParams.push_back({bp, cweight});
0174     bpMean = bpMean + mweight * bp;
0175   }
0176
0177   // Get the corrected bound covariance
0178   for (unsigned isample = 0; isample < sampleSize; ++isample) {
0179     BoundVector bSigma = transformedBoundParams[isample].first - bpMean;
0180
0181     bv = bv +
0182          transformedBoundParams[isample].second * bSigma * bSigma.transpose();
0183   }
0184
0185   return std::make_tuple(bpMean, bv);
0186 }