Acts/TrackFitting/GainMatrixSmoother.hpp

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 #pragma once
0010
0011 #include "Acts/EventData/MultiTrajectory.hpp"
0012 #include "Acts/Geometry/GeometryContext.hpp"
0013 #include "Acts/Utilities/Delegate.hpp"
0014 #include "Acts/Utilities/Logger.hpp"
0015 #include "Acts/Utilities/Result.hpp"
0016
0017 #include <cassert>
0018 #include <cstddef>
0019 #include <system_error>
0020
0021 namespace Acts {
0022
0023 /// Kalman trajectory smoother based on gain matrix formalism.
0024 ///
0025 /// This implements not a single smoothing step, but the full backwards
0026 /// smoothing procedure for a filtered, forward trajectory using the stored
0027 /// linearization.
0028 /// @ingroup track_fitting
0029 class GainMatrixSmoother {
0030  public:
0031   /// Whether to check the covariance matrices if they are semi-positive and if
0032   /// not attempt to correct them.
0033   bool doCovCheckAndAttemptFix = false;
0034
0035   /// Run the Kalman smoothing for one trajectory.
0036   ///
0037   /// @param[in] gctx The geometry context to be used
0038   /// @param[in,out] trajectory The trajectory to be smoothed
0039   /// @param[in] entryIndex The index of state to start the smoothing
0040   /// @param[in] logger Where to write logging information to
0041   /// @return Success or failure of the smoothing procedure
0042   template <typename traj_t>
0043   Result<void> operator()(const GeometryContext& gctx, traj_t& trajectory,
0044                           std::size_t entryIndex,
0045                           const Logger& logger = getDummyLogger()) const {
0046     (void)gctx;
0047
0048     using TrackStateProxy = typename traj_t::TrackStateProxy;
0049
0050     GetParameters filtered;
0051     GetCovariance filteredCovariance;
0052     GetParameters smoothed;
0053     GetParameters predicted;
0054     GetCovariance predictedCovariance;
0055     GetCovariance smoothedCovariance;
0056     GetCovariance jacobian;
0057
0058     filtered.connect([](const void*, void* ts) {
0059       return static_cast<TrackStateProxy*>(ts)->filtered();
0060     });
0061     filteredCovariance.connect([](const void*, void* ts) {
0062       return static_cast<TrackStateProxy*>(ts)->filteredCovariance();
0063     });
0064
0065     smoothed.connect([](const void*, void* ts) {
0066       return static_cast<TrackStateProxy*>(ts)->smoothed();
0067     });
0068     smoothedCovariance.connect([](const void*, void* ts) {
0069       return static_cast<TrackStateProxy*>(ts)->smoothedCovariance();
0070     });
0071
0072     predicted.connect([](const void*, void* ts) {
0073       return static_cast<TrackStateProxy*>(ts)->predicted();
0074     });
0075     predictedCovariance.connect([](const void*, void* ts) {
0076       return static_cast<TrackStateProxy*>(ts)->predictedCovariance();
0077     });
0078
0079     jacobian.connect([](const void*, void* ts) {
0080       return static_cast<TrackStateProxy*>(ts)->jacobian();
0081     });
0082
0083     ACTS_VERBOSE("Invoked GainMatrixSmoother on entry index: " << entryIndex);
0084
0085     // For the last state: smoothed is filtered - also: switch to next
0086     ACTS_VERBOSE("Getting previous track state");
0087     auto prev_ts = trajectory.getTrackState(entryIndex);
0088
0089     prev_ts.shareFrom(TrackStatePropMask::Filtered,
0090                       TrackStatePropMask::Smoothed);
0091
0092     // make sure there is more than one track state
0093     if (!prev_ts.hasPrevious()) {
0094       ACTS_VERBOSE("Only one track state given, smoothing terminates early");
0095       return Result<void>::success();
0096     }
0097
0098     ACTS_VERBOSE("Start smoothing from previous track state at index: "
0099                  << prev_ts.previous());
0100
0101     // default-constructed error represents success, i.e. an invalid error code
0102     std::error_code error;
0103     trajectory.applyBackwards(prev_ts.previous(), [&, this](auto ts) {
0104       // should have filtered and predicted, this should also include the
0105       // covariances.
0106       assert(ts.hasFiltered());
0107       assert(ts.hasPredicted());
0108
0109       // previous trackstate should have smoothed and predicted
0110       assert(prev_ts.hasSmoothed());
0111       assert(prev_ts.hasPredicted());
0112       assert(prev_ts.hasJacobian());
0113
0114       ACTS_VERBOSE("Calculate smoothing matrix:");
0115       ACTS_VERBOSE("Filtered covariance:\n" << ts.filteredCovariance());
0116       ACTS_VERBOSE("Jacobian:\n" << prev_ts.jacobian());
0117
0118       // ensure the track state has a smoothed component
0119       ts.addComponents(TrackStatePropMask::Smoothed);
0120
0121       if (auto res = calculate(&ts, &prev_ts, filtered, filteredCovariance,
0122                                smoothed, predicted, predictedCovariance,
0123                                smoothedCovariance, jacobian, logger);
0124           !res.ok()) {
0125         error = res.error();
0126         return false;
0127       }
0128
0129       prev_ts = ts;
0130       return true;  // continue execution
0131     });
0132
0133     return error ? Result<void>::failure(error) : Result<void>::success();
0134   }
0135
0136   /// Type alias for delegate to get track state parameters
0137   using GetParameters =
0138       Acts::Delegate<TrackStateTraits<kMeasurementSizeMax, false>::Parameters(
0139           void*)>;
0140   /// Type alias for delegate to get track state covariance matrix
0141   using GetCovariance =
0142       Acts::Delegate<TrackStateTraits<kMeasurementSizeMax, false>::Covariance(
0143           void*)>;
0144
0145   /// Calculate smoothed parameters for a single track state using gain matrix
0146   /// formalism.
0147   ///
0148   /// @param ts Current track state to be smoothed
0149   /// @param prev_ts Previous track state (already smoothed)
0150   /// @param filtered Delegate to get filtered parameters
0151   /// @param filteredCovariance Delegate to get filtered covariance
0152   /// @param smoothed Delegate to get smoothed parameters
0153   /// @param predicted Delegate to get predicted parameters
0154   /// @param predictedCovariance Delegate to get predicted covariance
0155   /// @param smoothedCovariance Delegate to get smoothed covariance
0156   /// @param jacobian Delegate to get Jacobian matrix
0157   /// @param logger Logger for verbose output
0158   /// @return Success or failure of the smoothing calculation
0159   Result<void> calculate(void* ts, void* prev_ts, const GetParameters& filtered,
0160                          const GetCovariance& filteredCovariance,
0161                          const GetParameters& smoothed,
0162                          const GetParameters& predicted,
0163                          const GetCovariance& predictedCovariance,
0164                          const GetCovariance& smoothedCovariance,
0165                          const GetCovariance& jacobian,
0166                          const Logger& logger) const;
0167 };
0168
0169 }  // namespace Acts