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