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   template <typename traj_t>
0041   Result<void> operator()(const GeometryContext& gctx, traj_t& trajectory,
0042                           std::size_t entryIndex,
0043                           const Logger& logger = getDummyLogger()) const {
0044     (void)gctx;
0045
0046     using TrackStateProxy = typename traj_t::TrackStateProxy;
0047
0048     GetParameters filtered;
0049     GetCovariance filteredCovariance;
0050     GetParameters smoothed;
0051     GetParameters predicted;
0052     GetCovariance predictedCovariance;
0053     GetCovariance smoothedCovariance;
0054     GetCovariance jacobian;
0055
0056     filtered.connect([](const void*, void* ts) {
0057       return static_cast<TrackStateProxy*>(ts)->filtered();
0058     });
0059     filteredCovariance.connect([](const void*, void* ts) {
0060       return static_cast<TrackStateProxy*>(ts)->filteredCovariance();
0061     });
0062
0063     smoothed.connect([](const void*, void* ts) {
0064       return static_cast<TrackStateProxy*>(ts)->smoothed();
0065     });
0066     smoothedCovariance.connect([](const void*, void* ts) {
0067       return static_cast<TrackStateProxy*>(ts)->smoothedCovariance();
0068     });
0069
0070     predicted.connect([](const void*, void* ts) {
0071       return static_cast<TrackStateProxy*>(ts)->predicted();
0072     });
0073     predictedCovariance.connect([](const void*, void* ts) {
0074       return static_cast<TrackStateProxy*>(ts)->predictedCovariance();
0075     });
0076
0077     jacobian.connect([](const void*, void* ts) {
0078       return static_cast<TrackStateProxy*>(ts)->jacobian();
0079     });
0080
0081     ACTS_VERBOSE("Invoked GainMatrixSmoother on entry index: " << entryIndex);
0082
0083     // For the last state: smoothed is filtered - also: switch to next
0084     ACTS_VERBOSE("Getting previous track state");
0085     auto prev_ts = trajectory.getTrackState(entryIndex);
0086
0087     prev_ts.shareFrom(TrackStatePropMask::Filtered,
0088                       TrackStatePropMask::Smoothed);
0089
0090     // make sure there is more than one track state
0091     if (!prev_ts.hasPrevious()) {
0092       ACTS_VERBOSE("Only one track state given, smoothing terminates early");
0093       return Result<void>::success();
0094     }
0095
0096     ACTS_VERBOSE("Start smoothing from previous track state at index: "
0097                  << prev_ts.previous());
0098
0099     // default-constructed error represents success, i.e. an invalid error code
0100     std::error_code error;
0101     trajectory.applyBackwards(prev_ts.previous(), [&, this](auto ts) {
0102       // should have filtered and predicted, this should also include the
0103       // covariances.
0104       assert(ts.hasFiltered());
0105       assert(ts.hasPredicted());
0106
0107       // previous trackstate should have smoothed and predicted
0108       assert(prev_ts.hasSmoothed());
0109       assert(prev_ts.hasPredicted());
0110       assert(prev_ts.hasJacobian());
0111
0112       ACTS_VERBOSE("Calculate smoothing matrix:");
0113       ACTS_VERBOSE("Filtered covariance:\n" << ts.filteredCovariance());
0114       ACTS_VERBOSE("Jacobian:\n" << prev_ts.jacobian());
0115
0116       // ensure the track state has a smoothed component
0117       ts.addComponents(TrackStatePropMask::Smoothed);
0118
0119       if (auto res = calculate(&ts, &prev_ts, filtered, filteredCovariance,
0120                                smoothed, predicted, predictedCovariance,
0121                                smoothedCovariance, jacobian, logger);
0122           !res.ok()) {
0123         error = res.error();
0124         return false;
0125       }
0126
0127       prev_ts = ts;
0128       return true;  // continue execution
0129     });
0130
0131     return error ? Result<void>::failure(error) : Result<void>::success();
0132   }
0133
0134   using GetParameters =
0135       Acts::Delegate<TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0136                                       false>::Parameters(void*)>;
0137   using GetCovariance =
0138       Acts::Delegate<TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0139                                       false>::Covariance(void*)>;
0140
0141   Result<void> calculate(void* ts, void* prev_ts, const GetParameters& filtered,
0142                          const GetCovariance& filteredCovariance,
0143                          const GetParameters& smoothed,
0144                          const GetParameters& predicted,
0145                          const GetCovariance& predictedCovariance,
0146                          const GetCovariance& smoothedCovariance,
0147                          const GetCovariance& jacobian,
0148                          const Logger& logger) const;
0149 };
0150
0151 }  // namespace Acts