src/Utilities/TrackHelpersUnbiased.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/AnyTrackStateProxy.hpp"
0010 #include "Acts/Utilities/Diagnostics.hpp"
0011 #include "Acts/Utilities/TrackHelpers.hpp"
0012
0013 // This translation unit exists solely to instantiate the (very expensive)
0014 // calculateUnbiasedParametersCovariance() once, against the type-erased
0015 // AnyConstTrackStateProxy. Callers use the non-template overload declared in
0016 // AnyTrackStateProxy.hpp and thus avoid re-instantiating the Eigen-heavy body
0017 // for their own concrete track state proxy type.
0018
0019 namespace Acts {
0020
0021 std::pair<BoundVector, BoundMatrix> calculateUnbiasedParametersCovariance(
0022     const AnyConstTrackStateProxy& trackState) {
0023   // Delegates to the deprecated templated implementation. This is the one
0024   // place it is meant to be instantiated, so silence the deprecation warning
0025   // for the internal call only.
0026   // ACTS_PUSH_IGNORE_DEPRECATED()
0027   // return calculateUnbiasedParametersCovariance<AnyConstTrackStateProxy>(
0028   //     trackState);
0029   // ACTS_POP_IGNORE_DEPRECATED()
0030
0031   if (!trackState.hasSmoothed()) {
0032     throw std::invalid_argument("track state has no smoothed parameters");
0033   }
0034   if (!trackState.hasCalibrated()) {
0035     throw std::invalid_argument("track state has no calibrated parameters");
0036   }
0037
0038   return visit_measurement(
0039       trackState.calibratedSize(),
0040       [&]<std::size_t measdim>(std::integral_constant<std::size_t, measdim>) {
0041         FixedBoundSubspaceHelper<measdim> subspaceHelper =
0042             trackState.template projectorSubspaceHelper<measdim>();
0043
0044         // TODO use subspace helper for projection instead
0045         auto H = subspaceHelper.projector();
0046         auto s = trackState.smoothed();
0047         auto C = trackState.smoothedCovariance();
0048         auto m = trackState.template calibrated<measdim>();
0049         auto V = trackState.template calibratedCovariance<measdim>();
0050         auto K =
0051             (C * H.transpose() * (H * C * H.transpose() - V).inverse()).eval();
0052         BoundVector unbiasedParamsVec = s + K * (m - H * s);
0053         BoundMatrix unbiasedParamsCov = C - K * H * C;
0054         return std::make_pair(unbiasedParamsVec, unbiasedParamsCov);
0055       });
0056 }
0057
0058 }  // namespace Acts