TrackFitting/detail/GsfUtils.hpp

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 #pragma once
0010
0011 #include "Acts/Definitions/Algebra.hpp"
0012 #include "Acts/Definitions/TrackParametrization.hpp"
0013 #include "Acts/EventData/MultiComponentTrackParameters.hpp"
0014 #include "Acts/EventData/MultiTrajectory.hpp"
0015 #include "Acts/EventData/TrackParameters.hpp"
0016 #include "Acts/EventData/Types.hpp"
0017 #include "Acts/Utilities/Logger.hpp"
0018
0019 #include <array>
0020 #include <cassert>
0021 #include <cmath>
0022 #include <cstddef>
0023 #include <iomanip>
0024 #include <map>
0025 #include <numeric>
0026 #include <ostream>
0027 #include <tuple>
0028 #include <vector>
0029
0030 namespace Acts {
0031
0032 /// The tolerated difference to 1 to accept weights as normalized
0033 constexpr static double s_normalizationTolerance = 1.e-4;
0034
0035 namespace detail {
0036
0037 template <typename component_range_t, typename projector_t>
0038 bool weightsAreNormalized(const component_range_t &cmps,
0039                           const projector_t &proj,
0040                           double tol = s_normalizationTolerance) {
0041   double sumOfWeights = 0.0;
0042
0043   for (auto it = cmps.begin(); it != cmps.end(); ++it) {
0044     sumOfWeights += proj(*it);
0045   }
0046
0047   return std::abs(sumOfWeights - 1.0) < tol;
0048 }
0049
0050 template <typename component_range_t, typename projector_t>
0051 void normalizeWeights(component_range_t &cmps, const projector_t &proj) {
0052   double sumOfWeights = 0.0;
0053
0054   // we need decltype(auto) here to support proxy-types with reference
0055   // semantics, otherwise there is a `cannot bind ... to ...` error
0056   for (auto it = cmps.begin(); it != cmps.end(); ++it) {
0057     decltype(auto) cmp = *it;
0058     assert(std::isfinite(proj(cmp)) && "weight not finite in normalization");
0059     sumOfWeights += proj(cmp);
0060   }
0061
0062   assert(sumOfWeights > 0 && "sum of weights is not > 0");
0063
0064   for (auto it = cmps.begin(); it != cmps.end(); ++it) {
0065     decltype(auto) cmp = *it;
0066     proj(cmp) /= sumOfWeights;
0067   }
0068 }
0069
0070 // A class that prints information about the state on construction and
0071 // destruction, it also contains some assertions in the constructor and
0072 // destructor. It can be removed without change of behaviour, since it only
0073 // holds const references
0074 template <typename propagator_state_t, typename stepper_t, typename navigator_t>
0075 class ScopedGsfInfoPrinterAndChecker {
0076   const propagator_state_t &m_state;
0077   const stepper_t &m_stepper;
0078   const navigator_t &m_navigator;
0079   double m_p_initial;
0080   const Logger &m_logger;
0081
0082   const Logger &logger() const { return m_logger; }
0083
0084   void print_component_stats() const {
0085     std::size_t i = 0;
0086     for (auto cmp : m_stepper.constComponentIterable(m_state.stepping)) {
0087       auto getVector = [&](auto idx) {
0088         return cmp.pars().template segment<3>(idx).transpose();
0089       };
0090       ACTS_VERBOSE("  #" << i++ << " pos: " << getVector(eFreePos0) << ", dir: "
0091                          << getVector(eFreeDir0) << ", weight: " << cmp.weight()
0092                          << ", status: " << cmp.status()
0093                          << ", qop: " << cmp.pars()[eFreeQOverP]
0094                          << ", det(cov): " << cmp.cov().determinant());
0095     }
0096   }
0097
0098   void checks(bool onStart) const {
0099     const auto cmps = m_stepper.constComponentIterable(m_state.stepping);
0100     [[maybe_unused]] const bool allFinite =
0101         std::all_of(cmps.begin(), cmps.end(),
0102                     [](auto cmp) { return std::isfinite(cmp.weight()); });
0103     [[maybe_unused]] const bool allNormalized = detail::weightsAreNormalized(
0104         cmps, [](const auto &cmp) { return cmp.weight(); });
0105     [[maybe_unused]] const bool zeroComponents =
0106         m_stepper.numberComponents(m_state.stepping) == 0;
0107
0108     if (onStart) {
0109       assert(!zeroComponents && "no cmps at the start");
0110       assert(allFinite && "weights not finite at the start");
0111       assert(allNormalized && "not normalized at the start");
0112     } else {
0113       assert(!zeroComponents && "no cmps at the end");
0114       assert(allFinite && "weights not finite at the end");
0115       assert(allNormalized && "not normalized at the end");
0116     }
0117   }
0118
0119  public:
0120   ScopedGsfInfoPrinterAndChecker(const propagator_state_t &state,
0121                                  const stepper_t &stepper,
0122                                  const navigator_t &navigator,
0123                                  const Logger &logger)
0124       : m_state(state),
0125         m_stepper(stepper),
0126         m_navigator(navigator),
0127         m_p_initial(stepper.absoluteMomentum(state.stepping)),
0128         m_logger{logger} {
0129     // Some initial printing
0130     checks(true);
0131     ACTS_VERBOSE("Gsf step "
0132                  << state.stepping.steps << " at mean position "
0133                  << stepper.position(state.stepping).transpose()
0134                  << " with direction "
0135                  << stepper.direction(state.stepping).transpose()
0136                  << " and momentum " << stepper.absoluteMomentum(state.stepping)
0137                  << " and charge " << stepper.charge(state.stepping));
0138     ACTS_VERBOSE("Propagation is in " << state.options.direction << " mode");
0139     print_component_stats();
0140   }
0141
0142   ~ScopedGsfInfoPrinterAndChecker() {
0143     if (m_navigator.currentSurface(m_state.navigation)) {
0144       const auto p_final = m_stepper.absoluteMomentum(m_state.stepping);
0145       ACTS_VERBOSE("Component status at end of step:");
0146       print_component_stats();
0147       ACTS_VERBOSE("Delta Momentum = " << std::setprecision(5)
0148                                        << p_final - m_p_initial);
0149     }
0150     checks(false);
0151   }
0152 };
0153
0154 double calculateDeterminant(
0155     const double *fullCalibratedCovariance,
0156     TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0157                      true>::Covariance predictedCovariance,
0158     BoundSubspaceIndices projector, unsigned int calibratedSize);
0159
0160 /// Reweight the components according to `R. Frühwirth, "Track fitting
0161 /// with non-Gaussian noise"`. See also the implementation in Athena at
0162 /// PosteriorWeightsCalculator.cxx
0163 /// @note The weights are not renormalized!
0164 template <typename traj_t>
0165 void computePosteriorWeights(
0166     const traj_t &mt, const std::vector<MultiTrajectoryTraits::IndexType> &tips,
0167     std::map<MultiTrajectoryTraits::IndexType, double> &weights) {
0168   // Helper Function to compute detR
0169
0170   // Find minChi2, this can be used to factor some things later in the
0171   // exponentiation
0172   const auto minChi2 =
0173       mt.getTrackState(*std::min_element(tips.begin(), tips.end(),
0174                                          [&](const auto &a, const auto &b) {
0175                                            return mt.getTrackState(a).chi2() <
0176                                                   mt.getTrackState(b).chi2();
0177                                          }))
0178           .chi2();
0179
0180   // Loop over the tips and compute new weights
0181   for (auto tip : tips) {
0182     const auto state = mt.getTrackState(tip);
0183     const double chi2 = state.chi2() - minChi2;
0184     const double detR = calculateDeterminant(
0185         // This abuses an incorrectly sized vector / matrix to access the
0186         // data pointer! This works (don't use the matrix as is!), but be
0187         // careful!
0188         state
0189             .template calibratedCovariance<
0190                 MultiTrajectoryTraits::MeasurementSizeMax>()
0191             .data(),
0192         state.predictedCovariance(), state.projectorSubspaceIndices(),
0193         state.calibratedSize());
0194
0195     if (detR <= 0) {
0196       // If the determinant is not positive, just leave the weight as it is
0197       continue;
0198     }
0199
0200     const auto factor = std::sqrt(1. / detR) * safeExp(-0.5 * chi2);
0201
0202     if (!std::isfinite(factor)) {
0203       // If something is not finite here, just leave the weight as it is
0204       continue;
0205     }
0206
0207     weights.at(tip) *= factor;
0208   }
0209 }
0210
0211 /// Enumeration type to allow templating on the state we want to project on with
0212 /// a MultiTrajectory
0213 enum class StatesType { ePredicted, eFiltered, eSmoothed };
0214
0215 inline std::ostream &operator<<(std::ostream &os, StatesType type) {
0216   constexpr static std::array names = {"predicted", "filtered", "smoothed"};
0217   os << names[static_cast<int>(type)];
0218   return os;
0219 }
0220
0221 /// @brief Projector type which maps a MultiTrajectory-Index to a tuple of
0222 /// [weight, parameters, covariance]. Therefore, it contains a MultiTrajectory
0223 /// and for now a std::map for the weights
0224 template <StatesType type, typename traj_t>
0225 struct MultiTrajectoryProjector {
0226   const traj_t &mt;
0227   const std::map<MultiTrajectoryTraits::IndexType, double> &weights;
0228
0229   auto operator()(MultiTrajectoryTraits::IndexType idx) const {
0230     const auto proxy = mt.getTrackState(idx);
0231     switch (type) {
0232       case StatesType::ePredicted:
0233         return std::make_tuple(weights.at(idx), proxy.predicted(),
0234                                proxy.predictedCovariance());
0235       case StatesType::eFiltered:
0236         return std::make_tuple(weights.at(idx), proxy.filtered(),
0237                                proxy.filteredCovariance());
0238       case StatesType::eSmoothed:
0239         return std::make_tuple(weights.at(idx), proxy.smoothed(),
0240                                proxy.smoothedCovariance());
0241       default:
0242         throw std::invalid_argument(
0243             "Incorrect StatesType, should be ePredicted"
0244             ", eFiltered, or eSmoothed.");
0245     }
0246   }
0247 };
0248
0249 /// Small Helper class that allows to carry a temporary value until we decide to
0250 /// update the actual value. The temporary value is deliberately only accessible
0251 /// with a mutable reference
0252 template <typename T>
0253 class Updatable {
0254   T m_tmp{};
0255   T m_val{};
0256
0257  public:
0258   Updatable() : m_tmp(0), m_val(0) {}
0259
0260   T &tmp() { return m_tmp; }
0261   void update() { m_val = m_tmp; }
0262
0263   const T &val() const { return m_val; }
0264 };
0265
0266 }  // namespace detail
0267 }  // namespace Acts