TrackFitting/detail/KalmanGlobalCovariance.hpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2020 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 http://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/MultiTrajectory.hpp"
0014 #include "Acts/EventData/TrackParameters.hpp"
0015 #include "Acts/Geometry/GeometryContext.hpp"
0016 #include "Acts/Surfaces/Surface.hpp"
0017
0018 #include <unordered_map>
0019
0020 namespace Acts::detail {
0021
0022 /// Calculate the global track parameters covariance for a smoothed trajectory
0023 /// stored in MultiTrajecty based on formulas at Journal of Physics: Conference
0024 /// Series 219 (2010) 032028.
0025 ///
0026 /// @tparam source_link_t The source link type of the trajectory
0027 /// @tparam parameters_t The track parameters type
0028 ///
0029 /// @param multiTraj The MultiTrajectory containing the trajectory to be
0030 /// investigated
0031 /// @param entryIndex The trajectory entry index
0032 ///
0033 /// @return The global track parameters covariance matrix and the starting
0034 /// row/column for smoothed states
0035 template <typename traj_t, typename parameters_t = BoundTrackParameters>
0036 std::pair<ActsDynamicMatrix, std::unordered_map<std::size_t, std::size_t>>
0037 globalTrackParametersCovariance(const traj_t& multiTraj,
0038                                 const std::size_t& entryIndex) {
0039   using CovMatrix = typename parameters_t::CovarianceMatrix;
0040   using GainMatrix = CovMatrix;
0041
0042   // The last smoothed state index
0043   std::size_t lastSmoothedIndex = SIZE_MAX;
0044   // The total number of smoothed states
0045   std::size_t nSmoothedStates = 0;
0046   // Visit all the states
0047   multiTraj.visitBackwards(entryIndex, [&](const auto& ts) {
0048     if (ts.hasSmoothed()) {
0049       if (lastSmoothedIndex == SIZE_MAX) {
0050         lastSmoothedIndex = ts.index();
0051       }
0052       nSmoothedStates++;
0053     }
0054   });
0055
0056   // Set the size of global track parameters covariance for all smoothed states
0057   ActsDynamicMatrix fullGlobalTrackParamsCov(nSmoothedStates * eBoundSize,
0058                                              nSmoothedStates * eBoundSize);
0059   fullGlobalTrackParamsCov.setZero();
0060   // The index of state within the trajectory and the starting row/column for
0061   // this state in the global covariance matrix
0062   std::unordered_map<std::size_t, std::size_t> stateRowIndices;
0063   // Visit the smoothed states to calculate the full global track parameters
0064   // covariance
0065   std::size_t nProcessed = 0;
0066   auto prev_ts = multiTraj.getTrackState(lastSmoothedIndex);
0067   multiTraj.visitBackwards(lastSmoothedIndex, [&](const auto& ts) {
0068     const std::size_t iRow =
0069         fullGlobalTrackParamsCov.rows() - eBoundSize * (nProcessed + 1);
0070     // Fill the covariance of this state
0071     fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(iRow, iRow) =
0072         ts.smoothedCovariance();
0073     // Fill the correlation between this state (indexed by i-1) and
0074     // beforehand smoothed states (indexed by j): C^n_{i-1, j}= G_{i-1} *
0075     // C^n_{i, j} for i <= j
0076     if (nProcessed > 0) {
0077       // Calculate the gain matrix
0078       GainMatrix G = ts.filteredCovariance() * prev_ts.jacobian().transpose() *
0079                      prev_ts.predictedCovariance().inverse();
0080       // Loop over the beforehand smoothed states
0081       for (std::size_t iProcessed = 1; iProcessed <= nProcessed; iProcessed++) {
0082         const std::size_t iCol = iRow + eBoundSize * iProcessed;
0083         CovMatrix prev_correlation =
0084             fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(
0085                 iRow + eBoundSize, iCol);
0086         CovMatrix correlation = G * prev_correlation;
0087         fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(iRow, iCol) =
0088             correlation;
0089         fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(iCol, iRow) =
0090             correlation.transpose();
0091       }
0092     }
0093     stateRowIndices.emplace(ts.index(), iRow);
0094     nProcessed++;
0095     prev_ts = ts;
0096   });
0097
0098   return std::make_pair(fullGlobalTrackParamsCov, stateRowIndices);
0099 }
0100
0101 }  // namespace Acts::detail