Acts/TrackFitting/GainMatrixSmoother.hpp

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