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