EventData/detail/covariance_helper.hpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2019 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/TrackParametrization.hpp"
0012
0013 #include <type_traits>
0014
0015 namespace Acts::detail {
0016 /// @brief check and correct covariance matrix
0017 ///
0018 /// @tparam CovMatrix_t The type of covariance matrix
0019 /// @tparam NumIter The number of iterations to run the correction
0020 ///
0021 /// Invocation:
0022 ///   - covariance_helper<CovMatrix_t, numIter>::validate(covariance)
0023 ///    The 'covariance' is checked against semi-positivedefiniteness
0024 ///    and limited number of iterations to replace it with the
0025 ///    closest semi-positivedefinite are made if it's not
0026 ///
0027 /// @return The (corrected) covariance is semi-positivedefinite or not
0028 template <typename CovMatrix_t, signed int NumIter = 1>
0029 struct covariance_helper {
0030   /// check if the covariance is semi-positive and correction is attempted
0031   static bool validate(CovMatrix_t& covariance) {
0032     if (covariance.hasNaN()) {
0033       return false;
0034     }
0035     std::size_t nIteration = 0;
0036     while (nIteration < NumIter) {
0037       if (isSemiPositive(covariance)) {
0038         return true;
0039       } else {
0040         Eigen::JacobiSVD<CovMatrix_t> svdCov(
0041             covariance, Eigen::ComputeFullU | Eigen::ComputeFullV);
0042         CovMatrix_t S = svdCov.singularValues().asDiagonal();
0043         CovMatrix_t V = svdCov.matrixV();
0044         CovMatrix_t H = V * S * V.transpose();
0045         covariance = (covariance + H) / 2;
0046         nIteration++;
0047       }
0048     }
0049     /// check again after the iterations
0050     return isSemiPositive(covariance);
0051   }
0052
0053   /// check if the covariance is semi-positive
0054   static bool isSemiPositive(const CovMatrix_t& covariance) {
0055     if (covariance.hasNaN()) {
0056       return false;
0057     }
0058     Eigen::LDLT<CovMatrix_t> ldltCov(covariance);
0059     return ldltCov.isPositive();
0060   }
0061
0062   /// check if the covariance is positive
0063   static bool isPositive(const CovMatrix_t& covariance) {
0064     if (covariance.hasNaN()) {
0065       return false;
0066     }
0067     Eigen::LLT<CovMatrix_t> lltCov(covariance);
0068     return lltCov.info() == Eigen::Success ? true : false;
0069   }
0070 };
0071
0072 }  // namespace Acts::detail