Mille/src/Helpers.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 "ActsPlugins/Mille/Helpers.hpp"
0010
0011 #include <iostream>
0012
0013 #include <Eigen/src/Core/Matrix.h>
0014 #include <Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h>
0015
0016 namespace ActsPlugins::ActsToMille {
0017
0018 Acts::DynamicMatrix regulariseCovariance(const Acts::DynamicMatrix& inputCov,
0019                                          double conditionCutOff,
0020                                          double removeHugeLeading,
0021                                          double stabilisationDiag) {
0022   Acts::DynamicMatrix out =
0023       Acts::DynamicMatrix::Zero(inputCov.rows(), inputCov.cols());
0024
0025   /// optionally add a tiny diagonal matrix for additional stabilisation
0026   Acts::DynamicMatrix regularisation =
0027       Acts::DynamicMatrix::Zero(inputCov.rows(), inputCov.cols());
0028   if (stabilisationDiag > 0) {
0029     regularisation = stabilisationDiag * Acts::DynamicMatrix::Identity(
0030                                              inputCov.rows(), inputCov.cols());
0031   }
0032   auto eigensolver = Eigen::SelfAdjointEigenSolver<Acts::DynamicMatrix>(
0033       inputCov + regularisation);
0034
0035   if (eigensolver.info() != Eigen::Success) {
0036     std::cout << " FAILED to find eigenvec" << std::endl;
0037     return out;
0038   }
0039   auto eigenVals = eigensolver.eigenvalues();
0040   auto eigenVecs = eigensolver.eigenvectors();
0041
0042   std::size_t maxIndex = eigenVals.size() - 1;
0043   double lambdaMax = eigenVals(eigenVals.size() - 1);
0044   // check for a huge leading eigenvalue - this happens when the time coordinate
0045   // is unconstrained
0046   if (removeHugeLeading > 0 &&
0047       lambdaMax > removeHugeLeading * eigenVals(maxIndex - 1)) {
0048     --maxIndex;
0049     lambdaMax = eigenVals(maxIndex);
0050   }
0051   double lambdaMin = eigenVals(0);
0052   if (conditionCutOff > 0) {
0053     lambdaMin = conditionCutOff * lambdaMax;
0054   }
0055   // clamp the EV to the permitted interval
0056   for (Eigen::Index i = 0; i < eigenVals.size(); ++i) {
0057     out(i, i) = std::clamp(eigenVals(i), lambdaMin, lambdaMax);
0058   }
0059   // then return the regularised matrix as V D V^T
0060   out = out * eigenVecs.transpose();
0061   out = eigenVecs * out;
0062   return out;
0063 }
0064
0065 Acts::DynamicMatrix getInverseComplement(
0066     const Acts::DynamicMatrix& target,
0067     const Acts::DynamicMatrix& existing_sol) {
0068   Acts::DynamicMatrix Rhs =
0069       Acts::DynamicMatrix::Identity(target.rows(), target.cols()) -
0070       target * existing_sol;
0071   // call decomposition from Eigen (prefer over llt for semi-def. matrices)
0072   auto LDLT = target.ldlt();
0073   Acts::DynamicMatrix solution = LDLT.solve(Rhs);
0074   // finally, symmetrise to correct for floating point effects
0075   solution = 0.5 * (solution + solution.transpose());
0076   return solution;
0077 }
0078
0079 }  // end namespace ActsPlugins::ActsToMille