TrackFitting/detail/Gx2fSumsBackend.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 // GX2F backend. One heavy Eigen decomposition per translation unit to keep
0010 // the peak compile memory low.
0011
0012 #include "Acts/TrackFitting/GlobalChiSquareFitterBackend.hpp"
0013 #include "Acts/Utilities/AlgebraHelpers.hpp"
0014
0015 void Acts::Experimental::addMeasurementToGx2fSumsBackend(
0016     Gx2fSystem& extendedSystem,
0017     const std::vector<BoundMatrix>& jacobianFromStart,
0018     const Eigen::MatrixXd& covarianceMeasurement, const BoundVector& predicted,
0019     const Eigen::VectorXd& measurement, const Eigen::MatrixXd& projector,
0020     const Logger& logger) {
0021   // First, w try to invert the covariance matrix. If the inversion fails, we
0022   // can already abort.
0023   const auto safeInvCovMeasurement = safeInverse(covarianceMeasurement);
0024   if (!safeInvCovMeasurement) {
0025     ACTS_WARNING("addMeasurementToGx2fSums: safeInvCovMeasurement failed.");
0026     ACTS_VERBOSE("    covarianceMeasurement:\n" << covarianceMeasurement);
0027     return;
0028   }
0029
0030   // Create an extended Jacobian. This one contains only eBoundSize rows,
0031   // because the rest is irrelevant. We fill it in the next steps.
0032   // TODO make dimsExtendedParams template with unrolling
0033   Eigen::MatrixXd extendedJacobian =
0034       Eigen::MatrixXd::Zero(eBoundSize, extendedSystem.nDims());
0035
0036   // This part of the Jacobian comes from the material-less propagation
0037   extendedJacobian.topLeftCorner<eBoundSize, eBoundSize>() =
0038       jacobianFromStart[0];
0039
0040   // If we have material, loop here over all Jacobians. We add extra columns for
0041   // their phi-theta projections. These parts account for the propagation of the
0042   // scattering angles.
0043   for (std::size_t matSurface = 1; matSurface < jacobianFromStart.size();
0044        matSurface++) {
0045     const BoundMatrix jac = jacobianFromStart[matSurface];
0046
0047     const Matrix<eBoundSize, 2> jacPhiTheta =
0048         jac * Gx2fConstants::phiThetaProjector;
0049
0050     // The position, where we need to insert the values in the extended Jacobian
0051     const std::size_t deltaPosition = eBoundSize + 2 * (matSurface - 1);
0052
0053     extendedJacobian.template block<eBoundSize, 2>(0, deltaPosition) =
0054         jacPhiTheta;
0055   }
0056
0057   const Eigen::MatrixXd projJacobian = projector * extendedJacobian;
0058
0059   const Eigen::VectorXd projPredicted = projector * predicted;
0060
0061   const Eigen::VectorXd residual = measurement - projPredicted;
0062
0063   // Finally contribute to chi2sum, aMatrix, and bVector
0064   extendedSystem.chi2() +=
0065       (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0);
0066
0067   extendedSystem.aMatrix() +=
0068       (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian)
0069           .eval();
0070
0071   extendedSystem.bVector() +=
0072       (residual.transpose() * (*safeInvCovMeasurement) * projJacobian)
0073           .eval()
0074           .transpose();
0075
0076   ACTS_VERBOSE(
0077       "Contributions in addMeasurementToGx2fSums:\n"
0078       << "    predicted:   " << predicted.transpose() << "\n"
0079       << "    measurement: " << measurement.transpose() << "\n"
0080       << "    covarianceMeasurement:\n"
0081       << covarianceMeasurement << "\n"
0082       << "    projector:\n"
0083       << projector.eval() << "\n"
0084       << "    projJacobian:\n"
0085       << projJacobian.eval() << "\n"
0086       << "    projPredicted: " << (projPredicted.transpose()).eval() << "\n"
0087       << "    residual: " << (residual.transpose()).eval() << "\n"
0088       << "    extendedJacobian:\n"
0089       << extendedJacobian << "\n"
0090       << "    aMatrix contribution:\n"
0091       << (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian)
0092              .eval()
0093       << "\n"
0094       << "    bVector contribution: "
0095       << (residual.transpose() * (*safeInvCovMeasurement) * projJacobian).eval()
0096       << "\n"
0097       << "    chi2sum contribution: "
0098       << (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0)
0099       << "\n"
0100       << "    safeInvCovMeasurement:\n"
0101       << (*safeInvCovMeasurement));
0102 }