From d5d6875705f1f7f1b83a41d05f16f34df5c7ff42 Mon Sep 17 00:00:00 2001 From: Paul Gessinger Date: Tue, 6 Aug 2024 15:35:51 +0200 Subject: [PATCH 1/5] refactor: `visit_measurement` can pipe through arguments --- Core/include/Acts/EventData/MeasurementHelpers.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Core/include/Acts/EventData/MeasurementHelpers.hpp b/Core/include/Acts/EventData/MeasurementHelpers.hpp index 10264d22595..71a0686e666 100644 --- a/Core/include/Acts/EventData/MeasurementHelpers.hpp +++ b/Core/include/Acts/EventData/MeasurementHelpers.hpp @@ -64,9 +64,10 @@ auto visit_measurement(A&& param, B&& cov, std::size_t dim, L&& lambda) { /// @param dim The runtime dimension of the measurement /// @param lambda The generic lambda instance to call /// @return Returns the lambda return value -template -auto visit_measurement(std::size_t dim, L&& lambda) { - return template_switch_lambda<1, eBoundSize>(dim, lambda); +template +auto visit_measurement(std::size_t dim, L&& lambda, Args&&... args) { + return template_switch_lambda<1, eBoundSize>(dim, lambda, + std::forward(args)...); } } // namespace Acts From fef6f807cb08fad60340beed7ed98262840cef1c Mon Sep 17 00:00:00 2001 From: Paul Gessinger Date: Tue, 6 Aug 2024 15:36:29 +0200 Subject: [PATCH 2/5] refactor: Split `GainMatrixUpdater` compilation --- .../Acts/TrackFitting/GainMatrixUpdater.hpp | 76 +++++++++++++++++++ Core/src/TrackFitting/CMakeLists.txt | 18 +++++ Core/src/TrackFitting/GainMatrixUpdater.cpp | 66 +++------------- .../TrackFitting/GainMatrixUpdaterImpl.cpp.in | 19 +++++ 4 files changed, 122 insertions(+), 57 deletions(-) create mode 100644 Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in diff --git a/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp b/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp index 86b6035bad8..44c49bd2c75 100644 --- a/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp +++ b/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp @@ -96,6 +96,82 @@ class GainMatrixUpdater { private: std::tuple visitMeasurement( InternalTrackState trackState, const Logger& logger) const; + + template + std::tuple visitMeasurementImpl( + InternalTrackState trackState, const Logger& logger) const; }; +template +std::tuple GainMatrixUpdater::visitMeasurementImpl( + InternalTrackState trackState, const Logger& logger) const { + double chi2 = 0; + + constexpr std::size_t kMeasurementSize = N; + using ParametersVector = ActsVector; + using CovarianceMatrix = ActsSquareMatrix; + + typename TrackStateTraits::Calibrated calibrated{ + trackState.calibrated}; + typename TrackStateTraits::CalibratedCovariance + calibratedCovariance{trackState.calibratedCovariance}; + + ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize); + ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose()); + ACTS_VERBOSE("Calibrated measurement covariance:\n" << calibratedCovariance); + + const auto H = trackState.projector + .template topLeftCorner() + .eval(); + + ACTS_VERBOSE("Measurement projector H:\n" << H); + + const auto K = (trackState.predictedCovariance * H.transpose() * + (H * trackState.predictedCovariance * H.transpose() + + calibratedCovariance) + .inverse()) + .eval(); + + ACTS_VERBOSE("Gain Matrix K:\n" << K); + + if (K.hasNaN()) { + // set to error abort execution + return {0, KalmanFitterError::UpdateFailed}; + } + + trackState.filtered = + trackState.predicted + K * (calibrated - H * trackState.predicted); + trackState.filteredCovariance = + (BoundSquareMatrix::Identity() - K * H) * trackState.predictedCovariance; + ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose()); + ACTS_VERBOSE("Filtered covariance:\n" << trackState.filteredCovariance); + + ParametersVector residual; + residual = calibrated - H * trackState.filtered; + ACTS_VERBOSE("Residual: " << residual.transpose()); + + CovarianceMatrix m = + ((CovarianceMatrix::Identity() - H * K) * calibratedCovariance).eval(); + + chi2 = (residual.transpose() * m.inverse() * residual).value(); + + ACTS_VERBOSE("Chi2: " << chi2); + + return {chi2, {}}; +} + +#define _EXTERN(N) \ + extern template std::tuple \ + GainMatrixUpdater::visitMeasurementImpl(InternalTrackState trackState, \ + const Logger& logger) const + +_EXTERN(1); +_EXTERN(2); +_EXTERN(3); +_EXTERN(4); +_EXTERN(5); +_EXTERN(6); + +#undef _EXTERN + } // namespace Acts diff --git a/Core/src/TrackFitting/CMakeLists.txt b/Core/src/TrackFitting/CMakeLists.txt index 922636b6d64..04ccbf3559e 100644 --- a/Core/src/TrackFitting/CMakeLists.txt +++ b/Core/src/TrackFitting/CMakeLists.txt @@ -12,3 +12,21 @@ target_sources( GlobalChiSquareFitter.cpp MbfSmoother.cpp ) + +foreach(DIM RANGE 1 6) + + set(dim_file ${CMAKE_CURRENT_BINARY_DIR}/GainMatrixUpdaterImpl${DIM}.cpp) + configure_file( + ${CMAKE_CURRENT_SOURCE_DIR}/GainMatrixUpdaterImpl.cpp.in + ${dim_file} + @ONLY + ) + + set_source_files_properties( + ${dim_file} + PROPERTIES COMPILE_DEFINITIONS ACTS_GAIN_MATRIX_UPDATER_INSTANTIATE=${DIM}) + target_sources(ActsCore + PRIVATE + ${dim_file}) + +endforeach() diff --git a/Core/src/TrackFitting/GainMatrixUpdater.cpp b/Core/src/TrackFitting/GainMatrixUpdater.cpp index 8fbd2b1d841..83d1ddfd2a3 100644 --- a/Core/src/TrackFitting/GainMatrixUpdater.cpp +++ b/Core/src/TrackFitting/GainMatrixUpdater.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -24,64 +25,15 @@ namespace Acts { std::tuple GainMatrixUpdater::visitMeasurement( InternalTrackState trackState, const Logger& logger) const { // default-constructed error represents success, i.e. an invalid error code - std::error_code error; - double chi2 = 0; - visit_measurement(trackState.calibratedSize, [&](auto N) -> void { - constexpr std::size_t kMeasurementSize = decltype(N)::value; - using ParametersVector = ActsVector; - using CovarianceMatrix = ActsSquareMatrix; - - typename TrackStateTraits::Calibrated calibrated{ - trackState.calibrated}; - typename TrackStateTraits::CalibratedCovariance - calibratedCovariance{trackState.calibratedCovariance}; - - ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize); - ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose()); - ACTS_VERBOSE("Calibrated measurement covariance:\n" - << calibratedCovariance); - - const auto H = trackState.projector - .template topLeftCorner() - .eval(); - - ACTS_VERBOSE("Measurement projector H:\n" << H); - - const auto K = (trackState.predictedCovariance * H.transpose() * - (H * trackState.predictedCovariance * H.transpose() + - calibratedCovariance) - .inverse()) - .eval(); - - ACTS_VERBOSE("Gain Matrix K:\n" << K); - - if (K.hasNaN()) { - // set to error abort execution - error = KalmanFitterError::UpdateFailed; - return; - } - - trackState.filtered = - trackState.predicted + K * (calibrated - H * trackState.predicted); - trackState.filteredCovariance = (BoundSquareMatrix::Identity() - K * H) * - trackState.predictedCovariance; - ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose()); - ACTS_VERBOSE("Filtered covariance:\n" << trackState.filteredCovariance); - - ParametersVector residual; - residual = calibrated - H * trackState.filtered; - ACTS_VERBOSE("Residual: " << residual.transpose()); - - CovarianceMatrix m = - ((CovarianceMatrix::Identity() - H * K) * calibratedCovariance).eval(); - - chi2 = (residual.transpose() * m.inverse() * residual).value(); - - ACTS_VERBOSE("Chi2: " << chi2); - }); - - return {chi2, error}; + return visit_measurement( + trackState.calibratedSize, + [this]( + std::integral_constant, InternalTrackState trackState, + const Logger& logger) -> std::tuple { + return visitMeasurementImpl(trackState, logger); + }, + std::move(trackState), logger); } } // namespace Acts diff --git a/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in b/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in new file mode 100644 index 00000000000..80a0300228e --- /dev/null +++ b/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in @@ -0,0 +1,19 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2024 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "Acts/TrackFitting/GainMatrixUpdater.hpp" + +// clang-format off + +namespace Acts { + +template std::tuple +GainMatrixUpdater::visitMeasurementImpl<@DIM@>(InternalTrackState trackState, + const Logger& logger) const; + +} // namespace Acts From d501de8d63d9cd6336d92cd42481fe49242461f2 Mon Sep 17 00:00:00 2001 From: Paul Gessinger Date: Tue, 6 Aug 2024 15:44:32 +0200 Subject: [PATCH 3/5] add missing doc comment --- Core/include/Acts/EventData/MeasurementHelpers.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Core/include/Acts/EventData/MeasurementHelpers.hpp b/Core/include/Acts/EventData/MeasurementHelpers.hpp index 71a0686e666..9d927e40447 100644 --- a/Core/include/Acts/EventData/MeasurementHelpers.hpp +++ b/Core/include/Acts/EventData/MeasurementHelpers.hpp @@ -63,6 +63,7 @@ auto visit_measurement(A&& param, B&& cov, std::size_t dim, L&& lambda) { /// @tparam L The generic lambda type to call /// @param dim The runtime dimension of the measurement /// @param lambda The generic lambda instance to call +/// @param args Additional arguments passed to @p lambda /// @return Returns the lambda return value template auto visit_measurement(std::size_t dim, L&& lambda, Args&&... args) { From 5f7b0086582d0b5fe67297d3e021054769e2fc5d Mon Sep 17 00:00:00 2001 From: Paul Gessinger Date: Tue, 6 Aug 2024 16:25:42 +0200 Subject: [PATCH 4/5] fix shadowing --- Core/src/TrackFitting/GainMatrixUpdater.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/Core/src/TrackFitting/GainMatrixUpdater.cpp b/Core/src/TrackFitting/GainMatrixUpdater.cpp index 83d1ddfd2a3..cbb50fcf5c4 100644 --- a/Core/src/TrackFitting/GainMatrixUpdater.cpp +++ b/Core/src/TrackFitting/GainMatrixUpdater.cpp @@ -28,12 +28,10 @@ std::tuple GainMatrixUpdater::visitMeasurement( return visit_measurement( trackState.calibratedSize, - [this]( - std::integral_constant, InternalTrackState trackState, - const Logger& logger) -> std::tuple { + [&, this](std::integral_constant) + -> std::tuple { return visitMeasurementImpl(trackState, logger); - }, - std::move(trackState), logger); + }); } } // namespace Acts From e0dddf42c465a29ef3d32d333e3f5a1599e37f90 Mon Sep 17 00:00:00 2001 From: Paul Gessinger Date: Tue, 6 Aug 2024 17:33:46 +0200 Subject: [PATCH 5/5] shuffle code location --- .../Acts/TrackFitting/GainMatrixUpdater.hpp | 72 -------------- .../detail/GainMatrixUpdaterImpl.hpp | 93 +++++++++++++++++++ .../TrackFitting/GainMatrixUpdaterImpl.cpp.in | 3 +- 3 files changed, 94 insertions(+), 74 deletions(-) create mode 100644 Core/include/Acts/TrackFitting/detail/GainMatrixUpdaterImpl.hpp diff --git a/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp b/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp index 44c49bd2c75..b5a49a6295d 100644 --- a/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp +++ b/Core/include/Acts/TrackFitting/GainMatrixUpdater.hpp @@ -102,76 +102,4 @@ class GainMatrixUpdater { InternalTrackState trackState, const Logger& logger) const; }; -template -std::tuple GainMatrixUpdater::visitMeasurementImpl( - InternalTrackState trackState, const Logger& logger) const { - double chi2 = 0; - - constexpr std::size_t kMeasurementSize = N; - using ParametersVector = ActsVector; - using CovarianceMatrix = ActsSquareMatrix; - - typename TrackStateTraits::Calibrated calibrated{ - trackState.calibrated}; - typename TrackStateTraits::CalibratedCovariance - calibratedCovariance{trackState.calibratedCovariance}; - - ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize); - ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose()); - ACTS_VERBOSE("Calibrated measurement covariance:\n" << calibratedCovariance); - - const auto H = trackState.projector - .template topLeftCorner() - .eval(); - - ACTS_VERBOSE("Measurement projector H:\n" << H); - - const auto K = (trackState.predictedCovariance * H.transpose() * - (H * trackState.predictedCovariance * H.transpose() + - calibratedCovariance) - .inverse()) - .eval(); - - ACTS_VERBOSE("Gain Matrix K:\n" << K); - - if (K.hasNaN()) { - // set to error abort execution - return {0, KalmanFitterError::UpdateFailed}; - } - - trackState.filtered = - trackState.predicted + K * (calibrated - H * trackState.predicted); - trackState.filteredCovariance = - (BoundSquareMatrix::Identity() - K * H) * trackState.predictedCovariance; - ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose()); - ACTS_VERBOSE("Filtered covariance:\n" << trackState.filteredCovariance); - - ParametersVector residual; - residual = calibrated - H * trackState.filtered; - ACTS_VERBOSE("Residual: " << residual.transpose()); - - CovarianceMatrix m = - ((CovarianceMatrix::Identity() - H * K) * calibratedCovariance).eval(); - - chi2 = (residual.transpose() * m.inverse() * residual).value(); - - ACTS_VERBOSE("Chi2: " << chi2); - - return {chi2, {}}; -} - -#define _EXTERN(N) \ - extern template std::tuple \ - GainMatrixUpdater::visitMeasurementImpl(InternalTrackState trackState, \ - const Logger& logger) const - -_EXTERN(1); -_EXTERN(2); -_EXTERN(3); -_EXTERN(4); -_EXTERN(5); -_EXTERN(6); - -#undef _EXTERN - } // namespace Acts diff --git a/Core/include/Acts/TrackFitting/detail/GainMatrixUpdaterImpl.hpp b/Core/include/Acts/TrackFitting/detail/GainMatrixUpdaterImpl.hpp new file mode 100644 index 00000000000..80b7729b835 --- /dev/null +++ b/Core/include/Acts/TrackFitting/detail/GainMatrixUpdaterImpl.hpp @@ -0,0 +1,93 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2024 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#pragma once + +#include "Acts/TrackFitting/GainMatrixUpdater.hpp" +#include "Acts/Utilities/Logger.hpp" + +#include +#include + +namespace Acts { + +template +std::tuple GainMatrixUpdater::visitMeasurementImpl( + InternalTrackState trackState, const Logger& logger) const { + double chi2 = 0; + + constexpr std::size_t kMeasurementSize = N; + using ParametersVector = ActsVector; + using CovarianceMatrix = ActsSquareMatrix; + + typename TrackStateTraits::Calibrated calibrated{ + trackState.calibrated}; + typename TrackStateTraits::CalibratedCovariance + calibratedCovariance{trackState.calibratedCovariance}; + + ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize); + ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose()); + ACTS_VERBOSE("Calibrated measurement covariance:\n" << calibratedCovariance); + + const auto H = trackState.projector + .template topLeftCorner() + .eval(); + + ACTS_VERBOSE("Measurement projector H:\n" << H); + + const auto K = (trackState.predictedCovariance * H.transpose() * + (H * trackState.predictedCovariance * H.transpose() + + calibratedCovariance) + .inverse()) + .eval(); + + ACTS_VERBOSE("Gain Matrix K:\n" << K); + + if (K.hasNaN()) { + // set to error abort execution + return {0, KalmanFitterError::UpdateFailed}; + } + + trackState.filtered = + trackState.predicted + K * (calibrated - H * trackState.predicted); + trackState.filteredCovariance = + (BoundSquareMatrix::Identity() - K * H) * trackState.predictedCovariance; + ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose()); + ACTS_VERBOSE("Filtered covariance:\n" << trackState.filteredCovariance); + + ParametersVector residual; + residual = calibrated - H * trackState.filtered; + ACTS_VERBOSE("Residual: " << residual.transpose()); + + CovarianceMatrix m = + ((CovarianceMatrix::Identity() - H * K) * calibratedCovariance).eval(); + + chi2 = (residual.transpose() * m.inverse() * residual).value(); + + ACTS_VERBOSE("Chi2: " << chi2); + + return {chi2, {}}; +} + +// Ensure thet the compiler does not implicitly instantiate the template + +#define _EXTERN(N) \ + extern template std::tuple \ + GainMatrixUpdater::visitMeasurementImpl(InternalTrackState trackState, \ + const Logger& logger) const + +_EXTERN(1); +_EXTERN(2); +_EXTERN(3); +_EXTERN(4); +_EXTERN(5); +_EXTERN(6); + +#undef _EXTERN + +} // namespace Acts diff --git a/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in b/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in index 80a0300228e..245de570cba 100644 --- a/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in +++ b/Core/src/TrackFitting/GainMatrixUpdaterImpl.cpp.in @@ -6,10 +6,9 @@ // License, v. 2.0. If a copy of the MPL was not distributed with this // file, You can obtain one at http://mozilla.org/MPL/2.0/. -#include "Acts/TrackFitting/GainMatrixUpdater.hpp" +#include "Acts/TrackFitting/detail/GainMatrixUpdaterImpl.hpp" // clang-format off - namespace Acts { template std::tuple