src/TrackFitting/GlobalChiSquareFitter.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/TrackFitting/GlobalChiSquareFitter.hpp"
0010
0011 #include "Acts/Definitions/TrackParametrization.hpp"
0012
0013 void Acts::Experimental::updateGx2fParams(
0014     BoundTrackParameters& params, const Eigen::VectorXd& deltaParamsExtended,
0015     const std::size_t nMaterialSurfaces,
0016     std::unordered_map<GeometryIdentifier, ScatteringProperties>& scatteringMap,
0017     const std::vector<GeometryIdentifier>& geoIdVector) {
0018   // update params
0019   params.parameters() +=
0020       deltaParamsExtended.topLeftCorner<eBoundSize, 1>().eval();
0021
0022   // update the scattering angles.
0023   for (std::size_t matSurface = 0; matSurface < nMaterialSurfaces;
0024        matSurface++) {
0025     const std::size_t deltaPosition = eBoundSize + 2 * matSurface;
0026     const GeometryIdentifier geoId = geoIdVector[matSurface];
0027     const auto scatteringMapId = scatteringMap.find(geoId);
0028     assert(scatteringMapId != scatteringMap.end() &&
0029            "No scattering angles found for material surface.");
0030     scatteringMapId->second.scatteringAngles().block<2, 1>(2, 0) +=
0031         deltaParamsExtended.block<2, 1>(deltaPosition, 0).eval();
0032   }
0033
0034   return;
0035 }
0036
0037 void Acts::Experimental::updateGx2fCovarianceParams(
0038     BoundMatrix& fullCovariancePredicted, Gx2fSystem& extendedSystem) {
0039   // make invertible
0040   for (std::size_t i = 0; i < extendedSystem.nDims(); ++i) {
0041     if (extendedSystem.aMatrix()(i, i) == 0.) {
0042       extendedSystem.aMatrix()(i, i) = 1.;
0043     }
0044   }
0045
0046   visit_measurement(extendedSystem.findRequiredNdf(), [&](auto N) {
0047     fullCovariancePredicted.topLeftCorner<N, N>() =
0048         extendedSystem.aMatrix().inverse().topLeftCorner<N, N>();
0049   });
0050
0051   return;
0052 }
0053
0054 void Acts::Experimental::addMeasurementToGx2fSumsBackend(
0055     Gx2fSystem& extendedSystem,
0056     const std::vector<BoundMatrix>& jacobianFromStart,
0057     const Eigen::MatrixXd& covarianceMeasurement, const BoundVector& predicted,
0058     const Eigen::VectorXd& measurement, const Eigen::MatrixXd& projector,
0059     const Logger& logger) {
0060   // First, w try to invert the covariance matrix. If the inversion fails, we
0061   // can already abort.
0062   const auto safeInvCovMeasurement = safeInverse(covarianceMeasurement);
0063   if (!safeInvCovMeasurement) {
0064     ACTS_WARNING("addMeasurementToGx2fSums: safeInvCovMeasurement failed.");
0065     ACTS_VERBOSE("    covarianceMeasurement:\n" << covarianceMeasurement);
0066     return;
0067   }
0068
0069   // Create an extended Jacobian. This one contains only eBoundSize rows,
0070   // because the rest is irrelevant. We fill it in the next steps.
0071   // TODO make dimsExtendedParams template with unrolling
0072   Eigen::MatrixXd extendedJacobian =
0073       Eigen::MatrixXd::Zero(eBoundSize, extendedSystem.nDims());
0074
0075   // This part of the Jacobian comes from the material-less propagation
0076   extendedJacobian.topLeftCorner<eBoundSize, eBoundSize>() =
0077       jacobianFromStart[0];
0078
0079   // If we have material, loop here over all Jacobians. We add extra columns for
0080   // their phi-theta projections. These parts account for the propagation of the
0081   // scattering angles.
0082   for (std::size_t matSurface = 1; matSurface < jacobianFromStart.size();
0083        matSurface++) {
0084     const BoundMatrix jac = jacobianFromStart[matSurface];
0085
0086     const ActsMatrix<eBoundSize, 2> jacPhiTheta =
0087         jac * Gx2fConstants::phiThetaProjector;
0088
0089     // The position, where we need to insert the values in the extended Jacobian
0090     const std::size_t deltaPosition = eBoundSize + 2 * (matSurface - 1);
0091
0092     extendedJacobian.template block<eBoundSize, 2>(0, deltaPosition) =
0093         jacPhiTheta;
0094   }
0095
0096   const Eigen::MatrixXd projJacobian = projector * extendedJacobian;
0097
0098   const Eigen::VectorXd projPredicted = projector * predicted;
0099
0100   const Eigen::VectorXd residual = measurement - projPredicted;
0101
0102   // Finally contribute to chi2sum, aMatrix, and bVector
0103   extendedSystem.chi2() +=
0104       (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0);
0105
0106   extendedSystem.aMatrix() +=
0107       (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian)
0108           .eval();
0109
0110   extendedSystem.bVector() +=
0111       (residual.transpose() * (*safeInvCovMeasurement) * projJacobian)
0112           .eval()
0113           .transpose();
0114
0115   ACTS_VERBOSE(
0116       "Contributions in addMeasurementToGx2fSums:\n"
0117       << "    predicted:   " << predicted.transpose() << "\n"
0118       << "    measurement: " << measurement.transpose() << "\n"
0119       << "    covarianceMeasurement:\n"
0120       << covarianceMeasurement << "\n"
0121       << "    projector:\n"
0122       << projector.eval() << "\n"
0123       << "    projJacobian:\n"
0124       << projJacobian.eval() << "\n"
0125       << "    projPredicted: " << (projPredicted.transpose()).eval() << "\n"
0126       << "    residual: " << (residual.transpose()).eval() << "\n"
0127       << "    extendedJacobian:\n"
0128       << extendedJacobian << "\n"
0129       << "    aMatrix contribution:\n"
0130       << (projJacobian.transpose() * (*safeInvCovMeasurement) * projJacobian)
0131              .eval()
0132       << "\n"
0133       << "    bVector contribution: "
0134       << (residual.transpose() * (*safeInvCovMeasurement) * projJacobian).eval()
0135       << "\n"
0136       << "    chi2sum contribution: "
0137       << (residual.transpose() * (*safeInvCovMeasurement) * residual)(0, 0)
0138       << "\n"
0139       << "    safeInvCovMeasurement:\n"
0140       << (*safeInvCovMeasurement));
0141 }
0142
0143 Eigen::VectorXd Acts::Experimental::computeGx2fDeltaParams(
0144     const Acts::Experimental::Gx2fSystem& extendedSystem) {
0145   return extendedSystem.aMatrix().colPivHouseholderQr().solve(
0146       extendedSystem.bVector());
0147 }