From 97947155816cfd8d8f0290171dd06bc227800f9a Mon Sep 17 00:00:00 2001 From: AJPfleger Date: Wed, 4 Dec 2024 15:31:29 +0100 Subject: [PATCH 1/4] addMeasurementToGx2fSumsBackend --- .../TrackFitting/GlobalChiSquareFitter.hpp | 111 +++++------------- .../include/Acts/Utilities/AlgebraHelpers.hpp | 3 +- .../TrackFitting/GlobalChiSquareFitter.cpp | 88 ++++++++++++++ 3 files changed, 116 insertions(+), 86 deletions(-) diff --git a/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp b/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp index dc0bc221e9a..297e4e30a2e 100644 --- a/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp +++ b/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp @@ -361,6 +361,29 @@ struct Gx2fSystem { std::size_t m_ndf = 0u; }; +/// @brief Adds a measurement to the GX2F equation system in a modular backend function. +/// +/// This function processes measurement data and integrates it into the GX2F +/// system. +/// +/// @param extendedSystem All parameters of the current equation system to update. +/// @param jacobianFromStart The Jacobian matrix from the start to the current state. +/// @param covarianceMeasurement The covariance matrix of the measurement. +/// @param predicted The predicted state vector based on the track state. +/// @param measurement The measurement vector. +/// @param projector The projection matrix. +/// @param logger A logger instance. +/// +/// @note The dynamic Eigen matrices are suboptimal. We could think of +/// templating again in the future on kMeasDims. We currently use dynamic +/// matrices to reduce the memory during compile time. +void addMeasurementToGx2fSumsBackend( + Gx2fSystem& extendedSystem, + const std::vector& jacobianFromStart, + const Eigen::MatrixXd& covarianceMeasurement, const BoundVector& predicted, + const Eigen::VectorXd& measurement, const Eigen::MatrixXd& projector, + const Logger& logger); + /// @brief Process measurements and fill the aMatrix and bVector /// /// The function processes each measurement for the GX2F Actor fitting process. @@ -370,7 +393,7 @@ struct Gx2fSystem { /// @tparam kMeasDim Number of dimensions of the measurement /// @tparam track_state_t The type of the track state /// -/// @param extendedSystem All parameters of the current equation system +/// @param extendedSystem All parameters of the current equation system to update /// @param jacobianFromStart The Jacobian matrix from start to the current state /// @param trackState The track state to analyse /// @param logger A logger instance @@ -379,44 +402,9 @@ void addMeasurementToGx2fSums(Gx2fSystem& extendedSystem, const std::vector& jacobianFromStart, const track_state_t& trackState, const Logger& logger) { - // First we get back the covariance and try to invert it. If the inversion - // fails, we can already abort. const ActsSquareMatrix covarianceMeasurement = trackState.template calibratedCovariance(); - const auto safeInvCovMeasurement = safeInverse(covarianceMeasurement); - if (!safeInvCovMeasurement) { - ACTS_WARNING("addMeasurementToGx2fSums: safeInvCovMeasurement failed."); - ACTS_VERBOSE(" covarianceMeasurement:\n" << covarianceMeasurement); - return; - } - - // Create an extended Jacobian. This one contains only eBoundSize rows, - // because the rest is irrelevant. We fill it in the next steps. - // TODO make dimsExtendedParams template with unrolling - Eigen::MatrixXd extendedJacobian = - Eigen::MatrixXd::Zero(eBoundSize, extendedSystem.nDims()); - - // This part of the Jacobian comes from the material-less propagation - extendedJacobian.topLeftCorner() = - jacobianFromStart[0]; - - // If we have material, loop here over all Jacobians. We add extra columns for - // their phi-theta projections. These parts account for the propagation of the - // scattering angles. - for (std::size_t matSurface = 1; matSurface < jacobianFromStart.size(); - matSurface++) { - const BoundMatrix jac = jacobianFromStart[matSurface]; - - const ActsMatrix jacPhiTheta = - jac * Gx2fConstants::phiThetaProjector; - - // The position, where we need to insert the values in the extended Jacobian - const std::size_t deltaPosition = eBoundSize + 2 * (matSurface - 1); - - extendedJacobian.block(0, deltaPosition) = jacPhiTheta; - } - const BoundVector predicted = trackState.smoothed(); const ActsVector measurement = @@ -425,54 +413,9 @@ void addMeasurementToGx2fSums(Gx2fSystem& extendedSystem, const ActsMatrix projector = trackState.template projectorSubspaceHelper().projector(); - const Eigen::MatrixXd projJacobian = projector * extendedJacobian; - - const ActsMatrix projPredicted = projector * predicted; - - const ActsVector residual = measurement - projPredicted; - - // Finally contribute to chi2sum, aMatrix, and bVector - extendedSystem.chi2() += - (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0); - - extendedSystem.aMatrix() += - (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian) - .eval(); - - extendedSystem.bVector() += - (residual.transpose() * (*safeInvCovMeasurement) * projJacobian) - .eval() - .transpose(); - - ACTS_VERBOSE( - "Contributions in addMeasurementToGx2fSums:\n" - << " kMeasDim: " << kMeasDim << "\n" - << " predicted: " << predicted.transpose() << "\n" - << " measurement: " << measurement.transpose() << "\n" - << " covarianceMeasurement:\n" - << covarianceMeasurement << "\n" - << " projector:\n" - << projector.eval() << "\n" - << " projJacobian:\n" - << projJacobian.eval() << "\n" - << " projPredicted: " << (projPredicted.transpose()).eval() << "\n" - << " residual: " << (residual.transpose()).eval() << "\n" - << " extendedJacobian:\n" - << extendedJacobian << "\n" - << " aMatrix contribution:\n" - << (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian) - .eval() - << "\n" - << " bVector contribution: " - << (residual.transpose() * (*safeInvCovMeasurement) * projJacobian).eval() - << "\n" - << " chi2sum contribution: " - << (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0) - << "\n" - << " safeInvCovMeasurement:\n" - << (*safeInvCovMeasurement)); - - return; + addMeasurementToGx2fSumsBackend(extendedSystem, jacobianFromStart, + covarianceMeasurement, predicted, measurement, + projector, logger); } /// @brief Process material and fill the aMatrix and bVector diff --git a/Core/include/Acts/Utilities/AlgebraHelpers.hpp b/Core/include/Acts/Utilities/AlgebraHelpers.hpp index 06379ec3559..dd3937ac341 100644 --- a/Core/include/Acts/Utilities/AlgebraHelpers.hpp +++ b/Core/include/Acts/Utilities/AlgebraHelpers.hpp @@ -192,12 +192,11 @@ std::optional safeInverse(const MatrixType& m) noexcept { constexpr int cols = MatrixType::ColsAtCompileTime; static_assert(rows == cols); - static_assert(rows != -1); ResultType result; bool invertible = false; - if constexpr (rows > 4) { + if constexpr (rows > 4 || rows == -1) { Eigen::FullPivLU mFullPivLU(m); if (mFullPivLU.isInvertible()) { invertible = true; diff --git a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp index 50b8b4cab8c..a8ca83fe3fd 100644 --- a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp +++ b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp @@ -50,3 +50,91 @@ void Acts::Experimental::updateGx2fCovarianceParams( return; } + +void Acts::Experimental::addMeasurementToGx2fSumsBackend( + Gx2fSystem& extendedSystem, + const std::vector& jacobianFromStart, + const Eigen::MatrixXd& covarianceMeasurement, const BoundVector& predicted, + const Eigen::VectorXd& measurement, const Eigen::MatrixXd& projector, + const Logger& logger) { + // First, w try to invert the covariance matrix. If the inversion fails, we + // can already abort. + const auto safeInvCovMeasurement = safeInverse(covarianceMeasurement); + if (!safeInvCovMeasurement) { + ACTS_WARNING("addMeasurementToGx2fSums: safeInvCovMeasurement failed."); + ACTS_VERBOSE(" covarianceMeasurement:\n" << covarianceMeasurement); + return; + } + + // Create an extended Jacobian. This one contains only eBoundSize rows, + // because the rest is irrelevant. We fill it in the next steps. + // TODO make dimsExtendedParams template with unrolling + Eigen::MatrixXd extendedJacobian = + Eigen::MatrixXd::Zero(eBoundSize, extendedSystem.nDims()); + + // This part of the Jacobian comes from the material-less propagation + extendedJacobian.topLeftCorner() = + jacobianFromStart[0]; + + // If we have material, loop here over all Jacobians. We add extra columns for + // their phi-theta projections. These parts account for the propagation of the + // scattering angles. + for (std::size_t matSurface = 1; matSurface < jacobianFromStart.size(); + matSurface++) { + const BoundMatrix jac = jacobianFromStart[matSurface]; + + const ActsMatrix jacPhiTheta = + jac * Gx2fConstants::phiThetaProjector; + + // The position, where we need to insert the values in the extended Jacobian + const std::size_t deltaPosition = eBoundSize + 2 * (matSurface - 1); + + extendedJacobian.block(0, deltaPosition) = jacPhiTheta; + } + + const Eigen::MatrixXd projJacobian = projector * extendedJacobian; + + const Eigen::VectorXd projPredicted = projector * predicted; + + const Eigen::VectorXd residual = measurement - projPredicted; + + // Finally contribute to chi2sum, aMatrix, and bVector + extendedSystem.chi2() += + (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0); + + extendedSystem.aMatrix() += + (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian) + .eval(); + + extendedSystem.bVector() += + (residual.transpose() * (*safeInvCovMeasurement) * projJacobian) + .eval() + .transpose(); + + ACTS_VERBOSE( + "Contributions in addMeasurementToGx2fSums:\n" + << " predicted: " << predicted.transpose() << "\n" + << " measurement: " << measurement.transpose() << "\n" + << " covarianceMeasurement:\n" + << covarianceMeasurement << "\n" + << " projector:\n" + << projector.eval() << "\n" + << " projJacobian:\n" + << projJacobian.eval() << "\n" + << " projPredicted: " << (projPredicted.transpose()).eval() << "\n" + << " residual: " << (residual.transpose()).eval() << "\n" + << " extendedJacobian:\n" + << extendedJacobian << "\n" + << " aMatrix contribution:\n" + << (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian) + .eval() + << "\n" + << " bVector contribution: " + << (residual.transpose() * (*safeInvCovMeasurement) * projJacobian).eval() + << "\n" + << " chi2sum contribution: " + << (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0) + << "\n" + << " safeInvCovMeasurement:\n" + << (*safeInvCovMeasurement)); +} From d08d08d7f5ec48fc54019ec7be927705c3536e27 Mon Sep 17 00:00:00 2001 From: AJPfleger Date: Thu, 5 Dec 2024 11:29:43 +0100 Subject: [PATCH 2/4] computeGx2fDeltaParams --- .../Acts/TrackFitting/GlobalChiSquareFitter.hpp | 17 +++++++++++------ Core/src/TrackFitting/GlobalChiSquareFitter.cpp | 6 ++++++ 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp b/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp index 297e4e30a2e..5e46bc0dba3 100644 --- a/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp +++ b/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp @@ -637,6 +637,15 @@ std::size_t countMaterialStates( return nMaterialSurfaces; } +/// @brief Solve the gx2f system to get the delta parameters for the update +/// +/// This function computes the delta parameters for the GX2F Actor fitting +/// process by solving the linear equation system [a] * delta = b. It uses the +/// column-pivoting Householder QR decomposition for numerical stability. +/// +/// @param extendedSystem All parameters of the current equation system +Eigen::VectorXd computeGx2fDeltaParams(const Gx2fSystem& extendedSystem); + /// @brief Update parameters (and scattering angles if applicable) /// /// @param params Parameters to be updated @@ -1334,10 +1343,8 @@ class Gx2Fitter { return Experimental::GlobalChiSquareFitterError::NotEnoughMeasurements; } - // calculate delta params [a] * delta = b Eigen::VectorXd deltaParamsExtended = - extendedSystem.aMatrix().colPivHouseholderQr().solve( - extendedSystem.bVector()); + computeGx2fDeltaParams(extendedSystem); ACTS_VERBOSE("aMatrix:\n" << extendedSystem.aMatrix() << "\n" @@ -1501,10 +1508,8 @@ class Gx2Fitter { return Experimental::GlobalChiSquareFitterError::NotEnoughMeasurements; } - // calculate delta params [a] * delta = b Eigen::VectorXd deltaParamsExtended = - extendedSystem.aMatrix().colPivHouseholderQr().solve( - extendedSystem.bVector()); + computeGx2fDeltaParams(extendedSystem); ACTS_VERBOSE("aMatrix:\n" << extendedSystem.aMatrix() << "\n" diff --git a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp index a8ca83fe3fd..3a74acb0b7b 100644 --- a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp +++ b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp @@ -138,3 +138,9 @@ void Acts::Experimental::addMeasurementToGx2fSumsBackend( << " safeInvCovMeasurement:\n" << (*safeInvCovMeasurement)); } + +Eigen::VectorXd Acts::Experimental::computeGx2fDeltaParams( + const Acts::Experimental::Gx2fSystem& extendedSystem) { + return extendedSystem.aMatrix().colPivHouseholderQr().solve( + extendedSystem.bVector()); +} From da902f7c6c8d10e16dbf39b533d54ffe54c04018 Mon Sep 17 00:00:00 2001 From: AJPfleger Date: Thu, 5 Dec 2024 17:23:06 +0100 Subject: [PATCH 3/4] try template for sonar --- Core/src/TrackFitting/GlobalChiSquareFitter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp index 3a74acb0b7b..811f5815146 100644 --- a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp +++ b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp @@ -89,7 +89,7 @@ void Acts::Experimental::addMeasurementToGx2fSumsBackend( // The position, where we need to insert the values in the extended Jacobian const std::size_t deltaPosition = eBoundSize + 2 * (matSurface - 1); - extendedJacobian.block(0, deltaPosition) = jacPhiTheta; + extendedJacobian.template block(0, deltaPosition) = jacPhiTheta; } const Eigen::MatrixXd projJacobian = projector * extendedJacobian; From 9c64eb797621472e76cdd7c411ee729617e91ed7 Mon Sep 17 00:00:00 2001 From: AJPfleger Date: Thu, 5 Dec 2024 21:01:06 +0100 Subject: [PATCH 4/4] format --- Core/src/TrackFitting/GlobalChiSquareFitter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp index 811f5815146..4062124496e 100644 --- a/Core/src/TrackFitting/GlobalChiSquareFitter.cpp +++ b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp @@ -89,7 +89,8 @@ void Acts::Experimental::addMeasurementToGx2fSumsBackend( // The position, where we need to insert the values in the extended Jacobian const std::size_t deltaPosition = eBoundSize + 2 * (matSurface - 1); - extendedJacobian.template block(0, deltaPosition) = jacPhiTheta; + extendedJacobian.template block(0, deltaPosition) = + jacPhiTheta; } const Eigen::MatrixXd projJacobian = projector * extendedJacobian;