pull out backend

AJPfleger · Dec 4, 2024 · 43e86df · 43e86df
1 parent af6a7b7
commit 43e86df
Show file tree

Hide file tree

Showing 2 changed files with 101 additions and 83 deletions.
diff --git a/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp b/Core/include/Acts/TrackFitting/GlobalChiSquareFitter.hpp
@@ -361,6 +361,15 @@ struct Gx2fSystem {
   std::size_t m_ndf = 0u;
 };
 
+// TODO write description
+template <std::size_t kMeasDim>
+void addMeasurementToGx2fSumsBackend(
+    Gx2fSystem& extendedSystem,
+    const std::vector<BoundMatrix>& jacobianFromStart,
+    const ActsSquareMatrix<kMeasDim>& covarianceMeasurement,
+    const BoundVector& predicted, const ActsVector<kMeasDim>& measurement,
+    const ActsMatrix<kMeasDim, eBoundSize>& projector, const Logger& logger);
+
 /// @brief Process measurements and fill the aMatrix and bVector
 ///
 /// The function processes each measurement for the GX2F Actor fitting process.
@@ -379,44 +388,9 @@ void addMeasurementToGx2fSums(Gx2fSystem& extendedSystem,
                               const std::vector<BoundMatrix>& 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<kMeasDim> covarianceMeasurement =
       trackState.template calibratedCovariance<kMeasDim>();
 
-  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<eBoundSize, eBoundSize>() =
-      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<eBoundSize, 2> 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<eBoundSize, 2>(0, deltaPosition) = jacPhiTheta;
-  }
-
   const BoundVector predicted = trackState.smoothed();
 
   const ActsVector<kMeasDim> measurement =
@@ -425,54 +399,9 @@ void addMeasurementToGx2fSums(Gx2fSystem& extendedSystem,
   const ActsMatrix<kMeasDim, eBoundSize> projector =
       trackState.template projectorSubspaceHelper<kMeasDim>().projector();
 
-  const Eigen::MatrixXd projJacobian = projector * extendedJacobian;
-
-  const ActsMatrix<kMeasDim, 1> projPredicted = projector * predicted;
-
-  const ActsVector<kMeasDim> 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/src/TrackFitting/GlobalChiSquareFitter.cpp b/Core/src/TrackFitting/GlobalChiSquareFitter.cpp
@@ -50,3 +50,92 @@ void Acts::Experimental::updateGx2fCovarianceParams(
 
   return;
 }
+
+void addMeasurementToGx2fSumsBackend(
+    Gx2fSystem& extendedSystem,
+    const std::vector<BoundMatrix>& jacobianFromStart,
+    const ActsSquareMatrix<kMeasDim>& covarianceMeasurement,
+    const BoundVector& predicted, const ActsVector<kMeasDim>& measurement,
+    const ActsMatrix<kMeasDim, eBoundSize>& 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<eBoundSize, eBoundSize>() =
+      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<eBoundSize, 2> 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<eBoundSize, 2>(0, deltaPosition) = jacPhiTheta;
+  }
+
+  const Eigen::MatrixXd projJacobian = projector * extendedJacobian;
+
+  const ActsMatrix<kMeasDim, 1> projPredicted = projector * predicted;
+
+  const ActsVector<kMeasDim> 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));
+}