Acts/TrackFitting/GainMatrixUpdater.hpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2016-2020 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 http://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/Definitions/Direction.hpp"
0012 #include "Acts/EventData/Measurement.hpp"
0013 #include "Acts/EventData/MeasurementHelpers.hpp"
0014 #include "Acts/EventData/MultiTrajectory.hpp"
0015 #include "Acts/Geometry/GeometryContext.hpp"
0016 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0017 #include "Acts/Utilities/Logger.hpp"
0018 #include "Acts/Utilities/Result.hpp"
0019
0020 #include <cassert>
0021 #include <system_error>
0022 #include <tuple>
0023
0024 namespace Acts {
0025
0026 /// Kalman update step using the gain matrix formalism.
0027 class GainMatrixUpdater {
0028   struct InternalTrackState {
0029     TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0030                      false>::Parameters predicted;
0031     TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0032                      false>::Covariance predictedCovariance;
0033     TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0034                      false>::Parameters filtered;
0035     TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0036                      false>::Covariance filteredCovariance;
0037     // This is used to build a covariance matrix view in the .cpp file
0038     double* calibrated;
0039     double* calibratedCovariance;
0040     TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0041                      false>::Projector projector;
0042     unsigned int calibratedSize;
0043   };
0044
0045  public:
0046   /// Run the Kalman update step for a single trajectory state.
0047   ///
0048   /// @tparam kMeasurementSizeMax
0049   /// @param[in,out] trackState The track state
0050   /// @param[in] direction The navigation direction
0051   /// @param[in] logger Where to write logging information to
0052   template <typename traj_t>
0053   Result<void> operator()(const GeometryContext& /*gctx*/,
0054                           typename traj_t::TrackStateProxy trackState,
0055                           Direction direction = Direction::Forward,
0056                           const Logger& logger = getDummyLogger()) const {
0057     ACTS_VERBOSE("Invoked GainMatrixUpdater");
0058
0059     // there should be a calibrated measurement
0060     assert(trackState.hasCalibrated());
0061     // we should have predicted state set
0062     assert(trackState.hasPredicted());
0063     // filtering should not have happened yet, but is allocated, therefore set
0064     assert(trackState.hasFiltered());
0065
0066     // read-only handles. Types are eigen maps to backing storage
0067     // const auto predicted = trackState.predicted();
0068     // const auto predictedCovariance = trackState.predictedCovariance();
0069
0070     ACTS_VERBOSE(
0071         "Predicted parameters: " << trackState.predicted().transpose());
0072     ACTS_VERBOSE("Predicted covariance:\n" << trackState.predictedCovariance());
0073
0074     // read-write handles. Types are eigen maps into backing storage.
0075     // This writes directly into the trajectory storage
0076     // auto filtered = trackState.filtered();
0077     // auto filteredCovariance = trackState.filteredCovariance();
0078
0079     auto [chi2, error] = visitMeasurement(
0080         InternalTrackState{
0081             trackState.predicted(),
0082             trackState.predictedCovariance(),
0083             trackState.filtered(),
0084             trackState.filteredCovariance(),
0085             // This abuses an incorrectly sized vector / matrix to access the
0086             // data pointer! This works (don't use the matrix as is!), but be
0087             // careful!
0088             trackState
0089                 .template calibrated<
0090                     MultiTrajectoryTraits::MeasurementSizeMax>()
0091                 .data(),
0092             trackState
0093                 .template calibratedCovariance<
0094                     MultiTrajectoryTraits::MeasurementSizeMax>()
0095                 .data(),
0096             trackState.projector(),
0097             trackState.calibratedSize(),
0098         },
0099         direction, logger);
0100
0101     trackState.chi2() = chi2;
0102
0103     return error ? Result<void>::failure(error) : Result<void>::success();
0104   }
0105
0106  private:
0107   std::tuple<double, std::error_code> visitMeasurement(
0108       InternalTrackState trackState, Direction direction,
0109       const Logger& logger) const;
0110 };
0111
0112 }  // namespace Acts