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