Back to home page

sPhenix code displayed by LXR

 
 

    


File indexing completed on 2025-08-06 08:10:14

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2016-2023 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 // Workaround for building on clang+libstdc++
0012 #include "Acts/Utilities/detail/ReferenceWrapperAnyCompat.hpp"
0013 
0014 #include "Acts/EventData/MultiTrajectory.hpp"
0015 #include "Acts/EventData/MultiTrajectoryHelpers.hpp"
0016 #include "Acts/EventData/SourceLink.hpp"
0017 #include "Acts/EventData/TrackHelpers.hpp"
0018 #include "Acts/EventData/TrackParameters.hpp"
0019 #include "Acts/EventData/VectorMultiTrajectory.hpp"
0020 #include "Acts/Geometry/GeometryContext.hpp"
0021 #include "Acts/MagneticField/MagneticFieldContext.hpp"
0022 #include "Acts/Propagator/AbortList.hpp"
0023 #include "Acts/Propagator/ActionList.hpp"
0024 #include "Acts/Propagator/DirectNavigator.hpp"
0025 #include "Acts/Propagator/Propagator.hpp"
0026 #include "Acts/Propagator/StandardAborters.hpp"
0027 #include "Acts/Propagator/detail/PointwiseMaterialInteraction.hpp"
0028 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0029 #include "Acts/TrackFitting/detail/KalmanUpdateHelpers.hpp"
0030 #include "Acts/TrackFitting/detail/VoidFitterComponents.hpp"
0031 #include "Acts/Utilities/CalibrationContext.hpp"
0032 #include "Acts/Utilities/Delegate.hpp"
0033 #include "Acts/Utilities/Logger.hpp"
0034 #include "Acts/Utilities/Result.hpp"
0035 
0036 #include <functional>
0037 #include <limits>
0038 #include <map>
0039 #include <memory>
0040 
0041 namespace Acts {
0042 
0043 enum class KalmanFitterTargetSurfaceStrategy {
0044   /// Use the first trackstate to reach target surface
0045   first,
0046   /// Use the last trackstate to reach target surface
0047   last,
0048   /// Use the first or last trackstate to reach target surface depending on the
0049   /// distance
0050   firstOrLast,
0051 };
0052 
0053 /// Extension struct which holds delegates to customise the KF behavior
0054 template <typename traj_t>
0055 struct KalmanFitterExtensions {
0056   using TrackStateProxy = typename traj_t::TrackStateProxy;
0057   using ConstTrackStateProxy = typename traj_t::ConstTrackStateProxy;
0058   using Parameters = typename TrackStateProxy::Parameters;
0059 
0060   using Calibrator =
0061       Delegate<void(const GeometryContext&, const CalibrationContext&,
0062                     const SourceLink&, TrackStateProxy)>;
0063 
0064   using Smoother = Delegate<Result<void>(const GeometryContext&, traj_t&,
0065                                          std::size_t, const Logger&)>;
0066 
0067   using Updater = Delegate<Result<void>(const GeometryContext&, TrackStateProxy,
0068                                         Direction, const Logger&)>;
0069 
0070   using OutlierFinder = Delegate<bool(ConstTrackStateProxy)>;
0071 
0072   using ReverseFilteringLogic = Delegate<bool(ConstTrackStateProxy)>;
0073 
0074   /// The Calibrator is a dedicated calibration algorithm that allows
0075   /// to calibrate measurements using track information, this could be
0076   /// e.g. sagging for wires, module deformations, etc.
0077   Calibrator calibrator;
0078 
0079   /// The updater incorporates measurement information into the track parameters
0080   Updater updater;
0081 
0082   /// The smoother back-propagates measurement information along the track
0083   Smoother smoother;
0084 
0085   /// Determines whether a measurement is supposed to be considered as an
0086   /// outlier
0087   OutlierFinder outlierFinder;
0088 
0089   /// Decides whether the smoothing stage uses linearized transport or full
0090   /// reverse propagation
0091   ReverseFilteringLogic reverseFilteringLogic;
0092 
0093   /// Retrieves the associated surface from a source link
0094   SourceLinkSurfaceAccessor surfaceAccessor;
0095 
0096   /// Default constructor which connects the default void components
0097   KalmanFitterExtensions() {
0098     calibrator.template connect<&detail::voidFitterCalibrator<traj_t>>();
0099     updater.template connect<&detail::voidFitterUpdater<traj_t>>();
0100     smoother.template connect<&detail::voidFitterSmoother<traj_t>>();
0101     outlierFinder.template connect<&detail::voidOutlierFinder<traj_t>>();
0102     reverseFilteringLogic
0103         .template connect<&detail::voidReverseFilteringLogic<traj_t>>();
0104     surfaceAccessor.connect<&detail::voidSurfaceAccessor>();
0105   }
0106 };
0107 
0108 /// Combined options for the Kalman fitter.
0109 ///
0110 /// @tparam traj_t The trajectory type
0111 template <typename traj_t>
0112 struct KalmanFitterOptions {
0113   /// PropagatorOptions with context.
0114   ///
0115   /// @param gctx The geometry context for this fit
0116   /// @param mctx The magnetic context for this fit
0117   /// @param cctx The calibration context for this fit
0118   /// @param extensions_ The KF extensions
0119   /// @param pOptions The plain propagator options
0120   /// @param rSurface The reference surface for the fit to be expressed at
0121   /// @param mScattering Whether to include multiple scattering
0122   /// @param eLoss Whether to include energy loss
0123   /// @param rFiltering Whether to run filtering in reversed direction as smoothing
0124   /// @param rfScaling Scale factor for the covariance matrix before the backward filtering
0125   /// @param freeToBoundCorrection_ Correction for non-linearity effect during transform from free to bound
0126   KalmanFitterOptions(const GeometryContext& gctx,
0127                       const MagneticFieldContext& mctx,
0128                       std::reference_wrapper<const CalibrationContext> cctx,
0129                       KalmanFitterExtensions<traj_t> extensions_,
0130                       const PropagatorPlainOptions& pOptions,
0131                       const Surface* rSurface = nullptr,
0132                       bool mScattering = true, bool eLoss = true,
0133                       bool rFiltering = false, double rfScaling = 1.0,
0134                       const FreeToBoundCorrection& freeToBoundCorrection_ =
0135                           FreeToBoundCorrection(false))
0136       : geoContext(gctx),
0137         magFieldContext(mctx),
0138         calibrationContext(cctx),
0139         extensions(extensions_),
0140         propagatorPlainOptions(pOptions),
0141         referenceSurface(rSurface),
0142         multipleScattering(mScattering),
0143         energyLoss(eLoss),
0144         reversedFiltering(rFiltering),
0145         reversedFilteringCovarianceScaling(rfScaling),
0146         freeToBoundCorrection(freeToBoundCorrection_) {}
0147   /// Contexts are required and the options must not be default-constructible.
0148   KalmanFitterOptions() = delete;
0149 
0150   /// Context object for the geometry
0151   std::reference_wrapper<const GeometryContext> geoContext;
0152   /// Context object for the magnetic field
0153   std::reference_wrapper<const MagneticFieldContext> magFieldContext;
0154   /// context object for the calibration
0155   std::reference_wrapper<const CalibrationContext> calibrationContext;
0156 
0157   KalmanFitterExtensions<traj_t> extensions;
0158 
0159   /// The trivial propagator options
0160   PropagatorPlainOptions propagatorPlainOptions;
0161 
0162   /// The reference Surface
0163   const Surface* referenceSurface = nullptr;
0164 
0165   /// Strategy to propagate to reference surface
0166   KalmanFitterTargetSurfaceStrategy referenceSurfaceStrategy =
0167       KalmanFitterTargetSurfaceStrategy::firstOrLast;
0168 
0169   /// Whether to consider multiple scattering
0170   bool multipleScattering = true;
0171 
0172   /// Whether to consider energy loss
0173   bool energyLoss = true;
0174 
0175   /// Whether to run filtering in reversed direction overwrite the
0176   /// ReverseFilteringLogic
0177   bool reversedFiltering = false;
0178 
0179   /// Factor by which the covariance of the input of the reversed filtering is
0180   /// scaled. This is only used in the backwardfiltering (if reversedFiltering
0181   /// is true or if the ReverseFilteringLogic return true for the track of
0182   /// interest)
0183   double reversedFilteringCovarianceScaling = 1.0;
0184 
0185   /// Whether to include non-linear correction during global to local
0186   /// transformation
0187   FreeToBoundCorrection freeToBoundCorrection;
0188 };
0189 
0190 template <typename traj_t>
0191 struct KalmanFitterResult {
0192   /// Fitted states that the actor has handled.
0193   traj_t* fittedStates{nullptr};
0194 
0195   /// This is the index of the 'tip' of the track stored in multitrajectory.
0196   /// This corresponds to the last measurement state in the multitrajectory.
0197   /// Since this KF only stores one trajectory, it is unambiguous.
0198   /// SIZE_MAX is the start of a trajectory.
0199   std::size_t lastMeasurementIndex = SIZE_MAX;
0200 
0201   /// This is the index of the 'tip' of the states stored in multitrajectory.
0202   /// This corresponds to the last state in the multitrajectory.
0203   /// Since this KF only stores one trajectory, it is unambiguous.
0204   /// SIZE_MAX is the start of a trajectory.
0205   std::size_t lastTrackIndex = SIZE_MAX;
0206 
0207   /// The optional Parameters at the provided surface
0208   std::optional<BoundTrackParameters> fittedParameters;
0209 
0210   /// Counter for states with non-outlier measurements
0211   std::size_t measurementStates = 0;
0212 
0213   /// Counter for measurements holes
0214   /// A hole correspond to a surface with an associated detector element with no
0215   /// associated measurement. Holes are only taken into account if they are
0216   /// between the first and last measurements.
0217   std::size_t measurementHoles = 0;
0218 
0219   /// Counter for handled states
0220   std::size_t processedStates = 0;
0221 
0222   /// Indicator if smoothing has been done.
0223   bool smoothed = false;
0224 
0225   /// Indicator if navigation direction has been reversed
0226   bool reversed = false;
0227 
0228   /// Indicator if track fitting has been done
0229   bool finished = false;
0230 
0231   /// Measurement surfaces without hits
0232   std::vector<const Surface*> missedActiveSurfaces;
0233 
0234   /// Measurement surfaces handled in both forward and backward filtering
0235   std::vector<const Surface*> passedAgainSurfaces;
0236 
0237   Result<void> result{Result<void>::success()};
0238 };
0239 
0240 /// Kalman fitter implementation.
0241 ///
0242 /// @tparam propagator_t Type of the propagation class
0243 ///
0244 /// The Kalman filter contains an Actor and a Sequencer sub-class.
0245 /// The Sequencer has to be part of the Navigator of the Propagator
0246 /// in order to initialize and provide the measurement surfaces.
0247 ///
0248 /// The Actor is part of the Propagation call and does the Kalman update
0249 /// and eventually the smoothing.  Updater, Smoother and Calibrator are
0250 /// given to the Actor for further use:
0251 /// - The Updater is the implemented kalman updater formalism, it
0252 ///   runs via a visitor pattern through the measurements.
0253 /// - The Smoother is called at the end of the filtering by the Actor.
0254 ///
0255 /// Measurements are not required to be ordered for the KalmanFilter,
0256 /// measurement ordering needs to be figured out by the navigation of
0257 /// the propagator.
0258 ///
0259 /// The void components are provided mainly for unit testing.
0260 template <typename propagator_t, typename traj_t>
0261 class KalmanFitter {
0262   /// The navigator type
0263   using KalmanNavigator = typename propagator_t::Navigator;
0264 
0265   /// The navigator has DirectNavigator type or not
0266   static constexpr bool isDirectNavigator =
0267       std::is_same<KalmanNavigator, DirectNavigator>::value;
0268 
0269  public:
0270   KalmanFitter(propagator_t pPropagator,
0271                std::unique_ptr<const Logger> _logger =
0272                    getDefaultLogger("KalmanFitter", Logging::INFO))
0273       : m_propagator(std::move(pPropagator)),
0274         m_logger{std::move(_logger)},
0275         m_actorLogger{m_logger->cloneWithSuffix("Actor")} {}
0276 
0277  private:
0278   /// The propagator for the transport and material update
0279   propagator_t m_propagator;
0280 
0281   /// The logger instance
0282   std::unique_ptr<const Logger> m_logger;
0283   std::unique_ptr<const Logger> m_actorLogger;
0284 
0285   const Logger& logger() const { return *m_logger; }
0286 
0287   /// @brief Propagator Actor plugin for the KalmanFilter
0288   ///
0289   /// @tparam parameters_t The type of parameters used for "local" parameters.
0290   /// @tparam calibrator_t The type of calibrator
0291   /// @tparam outlier_finder_t Type of the outlier finder class
0292   ///
0293   /// The KalmanActor does not rely on the measurements to be
0294   /// sorted along the track.
0295   template <typename parameters_t>
0296   class Actor {
0297    public:
0298     /// Broadcast the result_type
0299     using result_type = KalmanFitterResult<traj_t>;
0300 
0301     /// The target surface aboter
0302     SurfaceReached targetReached{std::numeric_limits<double>::lowest()};
0303 
0304     /// Strategy to propagate to target surface
0305     KalmanFitterTargetSurfaceStrategy targetSurfaceStrategy =
0306         KalmanFitterTargetSurfaceStrategy::firstOrLast;
0307 
0308     /// Allows retrieving measurements for a surface
0309     const std::map<GeometryIdentifier, SourceLink>* inputMeasurements = nullptr;
0310 
0311     /// Whether to consider multiple scattering.
0312     bool multipleScattering = true;
0313 
0314     /// Whether to consider energy loss.
0315     bool energyLoss = true;
0316 
0317     /// Whether run reversed filtering
0318     bool reversedFiltering = false;
0319 
0320     /// Scale the covariance before the reversed filtering
0321     double reversedFilteringCovarianceScaling = 1.0;
0322 
0323     /// Whether to include non-linear correction during global to local
0324     /// transformation
0325     FreeToBoundCorrection freeToBoundCorrection;
0326 
0327     /// Input MultiTrajectory
0328     std::shared_ptr<traj_t> outputStates;
0329 
0330     /// The logger instance
0331     const Logger* actorLogger{nullptr};
0332 
0333     /// Logger helper
0334     const Logger& logger() const { return *actorLogger; }
0335 
0336     KalmanFitterExtensions<traj_t> extensions;
0337 
0338     /// Calibration context for the fit
0339     const CalibrationContext* calibrationContext{nullptr};
0340 
0341     /// @brief Kalman actor operation
0342     ///
0343     /// @tparam propagator_state_t is the type of Propagator state
0344     /// @tparam stepper_t Type of the stepper
0345     /// @tparam navigator_t Type of the navigator
0346     ///
0347     /// @param state is the mutable propagator state object
0348     /// @param stepper The stepper in use
0349     /// @param navigator The navigator in use
0350     /// @param result is the mutable result state object
0351     template <typename propagator_state_t, typename stepper_t,
0352               typename navigator_t>
0353     void operator()(propagator_state_t& state, const stepper_t& stepper,
0354                     const navigator_t& navigator, result_type& result,
0355                     const Logger& /*logger*/) const {
0356       assert(result.fittedStates && "No MultiTrajectory set");
0357 
0358       if (result.finished) {
0359         return;
0360       }
0361 
0362       ACTS_VERBOSE("KalmanFitter step at pos: "
0363                    << stepper.position(state.stepping).transpose()
0364                    << " dir: " << stepper.direction(state.stepping).transpose()
0365                    << " momentum: "
0366                    << stepper.absoluteMomentum(state.stepping));
0367 
0368       // Add the measurement surface as external surface to navigator.
0369       // We will try to hit those surface by ignoring boundary checks.
0370       if constexpr (!isDirectNavigator) {
0371         if (result.processedStates == 0) {
0372           for (auto measurementIt = inputMeasurements->begin();
0373                measurementIt != inputMeasurements->end(); measurementIt++) {
0374             navigator.insertExternalSurface(state.navigation,
0375                                             measurementIt->first);
0376           }
0377         }
0378       }
0379 
0380       // Update:
0381       // - Waiting for a current surface
0382       auto surface = navigator.currentSurface(state.navigation);
0383       std::string direction = state.options.direction.toString();
0384       if (surface != nullptr) {
0385         // Check if the surface is in the measurement map
0386         // -> Get the measurement / calibrate
0387         // -> Create the predicted state
0388         // -> Check outlier behavior, if non-outlier:
0389         // -> Perform the kalman update
0390         // -> Fill track state information & update stepper information
0391 
0392         if (!result.smoothed && !result.reversed) {
0393           ACTS_VERBOSE("Perform " << direction << " filter step");
0394           auto res = filter(surface, state, stepper, navigator, result);
0395           if (!res.ok()) {
0396             ACTS_ERROR("Error in " << direction << " filter: " << res.error());
0397             result.result = res.error();
0398           }
0399         }
0400         if (result.reversed) {
0401           ACTS_VERBOSE("Perform " << direction << " filter step");
0402           auto res = reversedFilter(surface, state, stepper, navigator, result);
0403           if (!res.ok()) {
0404             ACTS_ERROR("Error in " << direction << " filter: " << res.error());
0405             result.result = res.error();
0406           }
0407         }
0408       }
0409 
0410       // Finalization:
0411       // when all track states have been handled or the navigation is breaked,
0412       // reset navigation&stepping before run reversed filtering or
0413       // proceed to run smoothing
0414       if (!result.smoothed && !result.reversed) {
0415         if (result.measurementStates == inputMeasurements->size() ||
0416             (result.measurementStates > 0 &&
0417              navigator.navigationBreak(state.navigation))) {
0418           // Remove the missing surfaces that occur after the last measurement
0419           result.missedActiveSurfaces.resize(result.measurementHoles);
0420           // now get track state proxy for the smoothing logic
0421           auto trackStateProxy =
0422               result.fittedStates->getTrackState(result.lastMeasurementIndex);
0423           if (reversedFiltering ||
0424               extensions.reverseFilteringLogic(trackStateProxy)) {
0425             // Start to run reversed filtering:
0426             // Reverse navigation direction and reset navigation and stepping
0427             // state to last measurement
0428             ACTS_VERBOSE("Reverse navigation direction.");
0429             auto res = reverse(state, stepper, navigator, result);
0430             if (!res.ok()) {
0431               ACTS_ERROR("Error in reversing navigation: " << res.error());
0432               result.result = res.error();
0433             }
0434           } else {
0435             // --> Search the starting state to run the smoothing
0436             // --> Call the smoothing
0437             // --> Set a stop condition when all track states have been
0438             // handled
0439             ACTS_VERBOSE("Finalize/run smoothing");
0440             auto res = finalize(state, stepper, navigator, result);
0441             if (!res.ok()) {
0442               ACTS_ERROR("Error in finalize: " << res.error());
0443               result.result = res.error();
0444             }
0445           }
0446         }
0447       }
0448 
0449       // Post-finalization:
0450       // - Progress to target/reference surface and built the final track
0451       // parameters
0452       if (result.smoothed || result.reversed) {
0453         if (result.smoothed) {
0454           // Update state and stepper with material effects
0455           // Only for smoothed as reverse filtering will handle this separately
0456           materialInteractor(navigator.currentSurface(state.navigation), state,
0457                              stepper, navigator,
0458                              MaterialUpdateStage::FullUpdate);
0459         }
0460 
0461         if (targetReached.surface == nullptr) {
0462           // If no target surface provided:
0463           // -> Return an error when using reversed filtering mode
0464           // -> Fitting is finished here
0465           if (result.reversed) {
0466             ACTS_ERROR(
0467                 "The target surface needed for aborting reversed propagation "
0468                 "is not provided");
0469             result.result =
0470                 Result<void>(KalmanFitterError::BackwardUpdateFailed);
0471           } else {
0472             ACTS_VERBOSE(
0473                 "No target surface set. Completing without fitted track "
0474                 "parameter");
0475             // Remember the track fitting is done
0476             result.finished = true;
0477           }
0478         } else if (targetReached(state, stepper, navigator, logger())) {
0479           ACTS_VERBOSE("Completing with fitted track parameter");
0480           // Transport & bind the parameter to the final surface
0481           auto res = stepper.boundState(state.stepping, *targetReached.surface,
0482                                         true, freeToBoundCorrection);
0483           if (!res.ok()) {
0484             ACTS_ERROR("Error in " << direction << " filter: " << res.error());
0485             result.result = res.error();
0486             return;
0487           }
0488           auto& fittedState = *res;
0489           // Assign the fitted parameters
0490           result.fittedParameters = std::get<BoundTrackParameters>(fittedState);
0491 
0492           // Reset smoothed status of states missed in reversed filtering
0493           if (result.reversed) {
0494             result.fittedStates->applyBackwards(
0495                 result.lastMeasurementIndex, [&](auto trackState) {
0496                   auto fSurface = &trackState.referenceSurface();
0497                   auto surface_it = std::find_if(
0498                       result.passedAgainSurfaces.begin(),
0499                       result.passedAgainSurfaces.end(),
0500                       [=](const Surface* s) { return s == fSurface; });
0501                   if (surface_it == result.passedAgainSurfaces.end()) {
0502                     // If reversed filtering missed this surface, then there is
0503                     // no smoothed parameter
0504                     trackState.unset(TrackStatePropMask::Smoothed);
0505                     trackState.typeFlags().set(TrackStateFlag::OutlierFlag);
0506                   }
0507                 });
0508           }
0509           // Remember the track fitting is done
0510           result.finished = true;
0511         }
0512       }
0513     }
0514 
0515     /// @brief Kalman actor operation: reverse direction
0516     ///
0517     /// @tparam propagator_state_t is the type of Propagator state
0518     /// @tparam stepper_t Type of the stepper
0519     /// @tparam navigator_t Type of the navigator
0520     ///
0521     /// @param state is the mutable propagator state object
0522     /// @param stepper The stepper in use
0523     /// @param navigator The navigator in use
0524     /// @param result is the mutable result state object
0525     template <typename propagator_state_t, typename stepper_t,
0526               typename navigator_t>
0527     Result<void> reverse(propagator_state_t& state, const stepper_t& stepper,
0528                          const navigator_t& navigator,
0529                          result_type& result) const {
0530       // Check if there is a measurement on track
0531       if (result.lastMeasurementIndex == SIZE_MAX) {
0532         ACTS_ERROR("No point to reverse for a track without measurements.");
0533         return KalmanFitterError::ReverseNavigationFailed;
0534       }
0535 
0536       // Remember the navigation direction has been reversed
0537       result.reversed = true;
0538 
0539       // Reverse navigation direction
0540       state.options.direction = state.options.direction.invert();
0541 
0542       // Reset propagator options
0543       // TODO Not sure if reset of pathLimit during propagation makes any sense
0544       state.options.pathLimit =
0545           state.options.direction * std::abs(state.options.pathLimit);
0546 
0547       // Get the last measurement state and reset navigation&stepping state
0548       // based on information on this state
0549       auto st = result.fittedStates->getTrackState(result.lastMeasurementIndex);
0550 
0551       // Update the stepping state
0552       stepper.resetState(
0553           state.stepping, st.filtered(),
0554           reversedFilteringCovarianceScaling * st.filteredCovariance(),
0555           st.referenceSurface(), state.options.maxStepSize);
0556 
0557       // For the last measurement state, smoothed is filtered
0558       st.smoothed() = st.filtered();
0559       st.smoothedCovariance() = st.filteredCovariance();
0560       result.passedAgainSurfaces.push_back(&st.referenceSurface());
0561 
0562       // Reset navigation state
0563       // We do not need to specify a target here since this will be handled
0564       // separately in the KF actor
0565       state.navigation = navigator.makeState(&st.referenceSurface(), nullptr);
0566       navigator.initialize(state, stepper);
0567 
0568       // Update material effects for last measurement state in reversed
0569       // direction
0570       materialInteractor(navigator.currentSurface(state.navigation), state,
0571                          stepper, navigator, MaterialUpdateStage::FullUpdate);
0572 
0573       return Result<void>::success();
0574     }
0575 
0576     /// @brief Kalman actor operation: update
0577     ///
0578     /// @tparam propagator_state_t is the type of Propagator state
0579     /// @tparam stepper_t Type of the stepper
0580     /// @tparam navigator_t Type of the navigator
0581     ///
0582     /// @param surface The surface where the update happens
0583     /// @param state The mutable propagator state object
0584     /// @param stepper The stepper in use
0585     /// @param navigator The navigator in use
0586     /// @param result The mutable result state object
0587     template <typename propagator_state_t, typename stepper_t,
0588               typename navigator_t>
0589     Result<void> filter(const Surface* surface, propagator_state_t& state,
0590                         const stepper_t& stepper, const navigator_t& navigator,
0591                         result_type& result) const {
0592       // Try to find the surface in the measurement surfaces
0593       auto sourcelink_it = inputMeasurements->find(surface->geometryId());
0594       if (sourcelink_it != inputMeasurements->end()) {
0595         // Screen output message
0596         ACTS_VERBOSE("Measurement surface " << surface->geometryId()
0597                                             << " detected.");
0598         // Transport the covariance to the surface
0599         stepper.transportCovarianceToBound(state.stepping, *surface,
0600                                            freeToBoundCorrection);
0601 
0602         // Update state and stepper with pre material effects
0603         materialInteractor(surface, state, stepper, navigator,
0604                            MaterialUpdateStage::PreUpdate);
0605 
0606         // do the kalman update (no need to perform covTransport here, hence no
0607         // point in performing globalToLocal correction)
0608         auto trackStateProxyRes = detail::kalmanHandleMeasurement(
0609             *calibrationContext, state, stepper, extensions, *surface,
0610             sourcelink_it->second, *result.fittedStates, result.lastTrackIndex,
0611             false, logger());
0612 
0613         if (!trackStateProxyRes.ok()) {
0614           return trackStateProxyRes.error();
0615         }
0616 
0617         const auto& trackStateProxy = *trackStateProxyRes;
0618         result.lastTrackIndex = trackStateProxy.index();
0619 
0620         // Update the stepper if it is not an outlier
0621         if (trackStateProxy.typeFlags().test(
0622                 Acts::TrackStateFlag::MeasurementFlag)) {
0623           // Update the stepping state with filtered parameters
0624           ACTS_VERBOSE("Filtering step successful, updated parameters are:\n"
0625                        << trackStateProxy.filtered().transpose());
0626           // update stepping state using filtered parameters after kalman
0627           stepper.update(state.stepping,
0628                          MultiTrajectoryHelpers::freeFiltered(
0629                              state.options.geoContext, trackStateProxy),
0630                          trackStateProxy.filtered(),
0631                          trackStateProxy.filteredCovariance(), *surface);
0632           // We count the state with measurement
0633           ++result.measurementStates;
0634         }
0635 
0636         // Update state and stepper with post material effects
0637         materialInteractor(surface, state, stepper, navigator,
0638                            MaterialUpdateStage::PostUpdate);
0639         // We count the processed state
0640         ++result.processedStates;
0641         // Update the number of holes count only when encountering a
0642         // measurement
0643         result.measurementHoles = result.missedActiveSurfaces.size();
0644         // Since we encountered a measurement update the lastMeasurementIndex to
0645         // the lastTrackIndex.
0646         result.lastMeasurementIndex = result.lastTrackIndex;
0647 
0648       } else if (surface->associatedDetectorElement() != nullptr ||
0649                  surface->surfaceMaterial() != nullptr) {
0650         // We only create track states here if there is already measurement
0651         // detected or if the surface has material (no holes before the first
0652         // measurement)
0653         if (result.measurementStates > 0 ||
0654             surface->surfaceMaterial() != nullptr) {
0655           auto trackStateProxyRes = detail::kalmanHandleNoMeasurement(
0656               state, stepper, *surface, *result.fittedStates,
0657               result.lastTrackIndex, true, logger(), freeToBoundCorrection);
0658 
0659           if (!trackStateProxyRes.ok()) {
0660             return trackStateProxyRes.error();
0661           }
0662 
0663           const auto& trackStateProxy = *trackStateProxyRes;
0664           result.lastTrackIndex = trackStateProxy.index();
0665 
0666           if (trackStateProxy.typeFlags().test(TrackStateFlag::HoleFlag)) {
0667             // Count the missed surface
0668             result.missedActiveSurfaces.push_back(surface);
0669           }
0670 
0671           ++result.processedStates;
0672         }
0673         if (surface->surfaceMaterial() != nullptr) {
0674           // Update state and stepper with material effects
0675           materialInteractor(surface, state, stepper, navigator,
0676                              MaterialUpdateStage::FullUpdate);
0677         }
0678       }
0679       return Result<void>::success();
0680     }
0681 
0682     /// @brief Kalman actor operation: update in reversed direction
0683     ///
0684     /// @tparam propagator_state_t is the type of Propagator state
0685     /// @tparam stepper_t Type of the stepper
0686     /// @tparam navigator_t Type of the navigator
0687     ///
0688     /// @param surface The surface where the update happens
0689     /// @param state The mutable propagator state object
0690     /// @param stepper The stepper in use
0691     /// @param navigator The navigator in use
0692     /// @param result The mutable result state object
0693     template <typename propagator_state_t, typename stepper_t,
0694               typename navigator_t>
0695     Result<void> reversedFilter(const Surface* surface,
0696                                 propagator_state_t& state,
0697                                 const stepper_t& stepper,
0698                                 const navigator_t& navigator,
0699                                 result_type& result) const {
0700       // Try to find the surface in the measurement surfaces
0701       auto sourcelink_it = inputMeasurements->find(surface->geometryId());
0702       if (sourcelink_it != inputMeasurements->end()) {
0703         // Screen output message
0704         ACTS_VERBOSE("Measurement surface "
0705                      << surface->geometryId()
0706                      << " detected in reversed propagation.");
0707 
0708         // No reversed filtering for last measurement state, but still update
0709         // with material effects
0710         if (result.reversed &&
0711             surface == navigator.startSurface(state.navigation)) {
0712           materialInteractor(surface, state, stepper, navigator,
0713                              MaterialUpdateStage::FullUpdate);
0714           return Result<void>::success();
0715         }
0716 
0717         // Transport the covariance to the surface
0718         stepper.transportCovarianceToBound(state.stepping, *surface,
0719                                            freeToBoundCorrection);
0720 
0721         // Update state and stepper with pre material effects
0722         materialInteractor(surface, state, stepper, navigator,
0723                            MaterialUpdateStage::PreUpdate);
0724 
0725         auto fittedStates = *result.fittedStates;
0726 
0727         // Add a <mask> TrackState entry multi trajectory. This allocates
0728         // storage for all components, which we will set later.
0729         TrackStatePropMask mask =
0730             TrackStatePropMask::Predicted | TrackStatePropMask::Filtered |
0731             TrackStatePropMask::Smoothed | TrackStatePropMask::Jacobian |
0732             TrackStatePropMask::Calibrated;
0733         const std::size_t currentTrackIndex = fittedStates.addTrackState(
0734             mask, Acts::MultiTrajectoryTraits::kInvalid);
0735 
0736         // now get track state proxy back
0737         typename traj_t::TrackStateProxy trackStateProxy =
0738             fittedStates.getTrackState(currentTrackIndex);
0739 
0740         // Set the trackStateProxy components with the state from the ongoing
0741         // propagation
0742         {
0743           trackStateProxy.setReferenceSurface(surface->getSharedPtr());
0744           // Bind the transported state to the current surface
0745           auto res = stepper.boundState(state.stepping, *surface, false,
0746                                         freeToBoundCorrection);
0747           if (!res.ok()) {
0748             return res.error();
0749           }
0750           const auto& [boundParams, jacobian, pathLength] = *res;
0751 
0752           // Fill the track state
0753           trackStateProxy.predicted() = boundParams.parameters();
0754           trackStateProxy.predictedCovariance() = state.stepping.cov;
0755 
0756           trackStateProxy.jacobian() = jacobian;
0757           trackStateProxy.pathLength() = pathLength;
0758         }
0759 
0760         // We have predicted parameters, so calibrate the uncalibrated input
0761         // measurement
0762         extensions.calibrator(state.geoContext, *calibrationContext,
0763                               sourcelink_it->second, trackStateProxy);
0764 
0765         // If the update is successful, set covariance and
0766         auto updateRes = extensions.updater(state.geoContext, trackStateProxy,
0767                                             state.options.direction, logger());
0768         if (!updateRes.ok()) {
0769           ACTS_ERROR("Backward update step failed: " << updateRes.error());
0770           return updateRes.error();
0771         } else {
0772           // Update the stepping state with filtered parameters
0773           ACTS_VERBOSE(
0774               "Backward filtering step successful, updated parameters are:\n"
0775               << trackStateProxy.filtered().transpose());
0776 
0777           // Fill the smoothed parameter for the existing track state
0778           result.fittedStates->applyBackwards(
0779               result.lastMeasurementIndex, [&](auto trackState) {
0780                 auto fSurface = &trackState.referenceSurface();
0781                 if (fSurface == surface) {
0782                   result.passedAgainSurfaces.push_back(surface);
0783                   trackState.smoothed() = trackStateProxy.filtered();
0784                   trackState.smoothedCovariance() =
0785                       trackStateProxy.filteredCovariance();
0786                   return false;
0787                 }
0788                 return true;
0789               });
0790 
0791           // update stepping state using filtered parameters after kalman
0792           // update We need to (re-)construct a BoundTrackParameters instance
0793           // here, which is a bit awkward.
0794           stepper.update(state.stepping,
0795                          MultiTrajectoryHelpers::freeFiltered(
0796                              state.options.geoContext, trackStateProxy),
0797                          trackStateProxy.filtered(),
0798                          trackStateProxy.filteredCovariance(), *surface);
0799 
0800           // Update state and stepper with post material effects
0801           materialInteractor(surface, state, stepper, navigator,
0802                              MaterialUpdateStage::PostUpdate);
0803         }
0804       } else if (surface->associatedDetectorElement() != nullptr ||
0805                  surface->surfaceMaterial() != nullptr) {
0806         // Transport covariance
0807         if (surface->associatedDetectorElement() != nullptr) {
0808           ACTS_VERBOSE("Detected hole on " << surface->geometryId()
0809                                            << " in reversed filtering");
0810           if (state.stepping.covTransport) {
0811             stepper.transportCovarianceToBound(state.stepping, *surface);
0812           }
0813         } else if (surface->surfaceMaterial() != nullptr) {
0814           ACTS_VERBOSE("Detected in-sensitive surface "
0815                        << surface->geometryId() << " in reversed filtering");
0816           if (state.stepping.covTransport) {
0817             stepper.transportCovarianceToCurvilinear(state.stepping);
0818           }
0819         }
0820         // Not creating bound state here, so need manually reinitialize
0821         // jacobian
0822         stepper.setIdentityJacobian(state.stepping);
0823         if (surface->surfaceMaterial() != nullptr) {
0824           // Update state and stepper with material effects
0825           materialInteractor(surface, state, stepper, navigator,
0826                              MaterialUpdateStage::FullUpdate);
0827         }
0828       }
0829 
0830       return Result<void>::success();
0831     }
0832 
0833     /// @brief Kalman actor operation: material interaction
0834     ///
0835     /// @tparam propagator_state_t is the type of Propagator state
0836     /// @tparam stepper_t Type of the stepper
0837     /// @tparam navigator_t Type of the navigator
0838     ///
0839     /// @param surface The surface where the material interaction happens
0840     /// @param state The mutable propagator state object
0841     /// @param stepper The stepper in use
0842     /// @param navigator The navigator in use
0843     /// @param updateStage The material update stage
0844     ///
0845     template <typename propagator_state_t, typename stepper_t,
0846               typename navigator_t>
0847     void materialInteractor(const Surface* surface, propagator_state_t& state,
0848                             const stepper_t& stepper,
0849                             const navigator_t& navigator,
0850                             const MaterialUpdateStage& updateStage) const {
0851       // Protect against null surface
0852       if (!surface) {
0853         ACTS_VERBOSE(
0854             "Surface is nullptr. Cannot be used for material interaction");
0855         return;
0856       }
0857 
0858       // Indicator if having material
0859       bool hasMaterial = false;
0860 
0861       if (surface && surface->surfaceMaterial()) {
0862         // Prepare relevant input particle properties
0863         detail::PointwiseMaterialInteraction interaction(surface, state,
0864                                                          stepper);
0865         // Evaluate the material properties
0866         if (interaction.evaluateMaterialSlab(state, navigator, updateStage)) {
0867           // Surface has material at this stage
0868           hasMaterial = true;
0869 
0870           // Evaluate the material effects
0871           interaction.evaluatePointwiseMaterialInteraction(multipleScattering,
0872                                                            energyLoss);
0873 
0874           // Screen out material effects info
0875           ACTS_VERBOSE("Material effects on surface: "
0876                        << surface->geometryId()
0877                        << " at update stage: " << updateStage << " are :");
0878           ACTS_VERBOSE("eLoss = "
0879                        << interaction.Eloss << ", "
0880                        << "variancePhi = " << interaction.variancePhi << ", "
0881                        << "varianceTheta = " << interaction.varianceTheta
0882                        << ", "
0883                        << "varianceQoverP = " << interaction.varianceQoverP);
0884 
0885           // Update the state and stepper with material effects
0886           interaction.updateState(state, stepper, addNoise);
0887         }
0888       }
0889 
0890       if (!hasMaterial) {
0891         // Screen out message
0892         //ACTS_VERBOSE("No material effects on surface: " << surface->geometryId()
0893     //                                              << " at update stage: "
0894         //                                                << updateStage);
0895       }
0896     }
0897 
0898     /// @brief Kalman actor operation: finalize
0899     ///
0900     /// @tparam propagator_state_t is the type of Propagator state
0901     /// @tparam stepper_t Type of the stepper
0902     /// @tparam navigator_t Type of the navigator
0903     ///
0904     /// @param state is the mutable propagator state object
0905     /// @param stepper The stepper in use
0906     /// @param navigator The navigator in use
0907     /// @param result is the mutable result state object
0908     template <typename propagator_state_t, typename stepper_t,
0909               typename navigator_t>
0910     Result<void> finalize(propagator_state_t& state, const stepper_t& stepper,
0911                           const navigator_t& navigator,
0912                           result_type& result) const {
0913       // Remember you smoothed the track states
0914       result.smoothed = true;
0915 
0916       // Get the indices of the first states (can be either a measurement or
0917       // material);
0918       std::size_t firstStateIndex = result.lastMeasurementIndex;
0919       // Count track states to be smoothed
0920       std::size_t nStates = 0;
0921       result.fittedStates->applyBackwards(
0922           result.lastMeasurementIndex, [&](auto st) {
0923             bool isMeasurement =
0924                 st.typeFlags().test(TrackStateFlag::MeasurementFlag);
0925             bool isOutlier = st.typeFlags().test(TrackStateFlag::OutlierFlag);
0926             // We are excluding non measurement states and outlier here. Those
0927             // can decrease resolution because only the smoothing corrected the
0928             // very first prediction as filtering is not possible.
0929             if (isMeasurement && !isOutlier) {
0930               firstStateIndex = st.index();
0931             }
0932             nStates++;
0933           });
0934       // Return error if the track has no measurement states (but this should
0935       // not happen)
0936       if (nStates == 0) {
0937         ACTS_ERROR("Smoothing for a track without measurements.");
0938         return KalmanFitterError::SmoothFailed;
0939       }
0940       // Screen output for debugging
0941       ACTS_VERBOSE("Apply smoothing on " << nStates
0942                                          << " filtered track states.");
0943 
0944       // Smooth the track states
0945       auto smoothRes =
0946           extensions.smoother(state.geoContext, *result.fittedStates,
0947                               result.lastMeasurementIndex, logger());
0948       if (!smoothRes.ok()) {
0949         ACTS_ERROR("Smoothing step failed: " << smoothRes.error());
0950         return smoothRes.error();
0951       }
0952 
0953       // Return in case no target surface
0954       if (targetReached.surface == nullptr) {
0955         return Result<void>::success();
0956       }
0957 
0958       // Obtain the smoothed parameters at first/last measurement state
0959       auto firstCreatedState =
0960           result.fittedStates->getTrackState(firstStateIndex);
0961       auto lastCreatedMeasurement =
0962           result.fittedStates->getTrackState(result.lastMeasurementIndex);
0963 
0964       // Lambda to get the intersection of the free params on the target surface
0965       auto target = [&](const FreeVector& freeVector) -> SurfaceIntersection {
0966         return targetReached.surface
0967             ->intersect(
0968                 state.geoContext, freeVector.segment<3>(eFreePos0),
0969                 state.options.direction * freeVector.segment<3>(eFreeDir0),
0970                 BoundaryCheck(true), state.options.surfaceTolerance)
0971             .closest();
0972       };
0973 
0974       // The smoothed free params at the first/last measurement state.
0975       // (the first state can also be a material state)
0976       auto firstParams = MultiTrajectoryHelpers::freeSmoothed(
0977           state.options.geoContext, firstCreatedState);
0978       auto lastParams = MultiTrajectoryHelpers::freeSmoothed(
0979           state.options.geoContext, lastCreatedMeasurement);
0980       // Get the intersections of the smoothed free parameters with the target
0981       // surface
0982       const auto firstIntersection = target(firstParams);
0983       const auto lastIntersection = target(lastParams);
0984 
0985       // Update the stepping parameters - in order to progress to destination.
0986       // At the same time, reverse navigation direction for further stepping if
0987       // necessary.
0988       // @note The stepping parameters is updated to the smoothed parameters at
0989       // either the first measurement state or the last measurement state. It
0990       // assumes the target surface is not within the first and the last
0991       // smoothed measurement state. Also, whether the intersection is on
0992       // surface is not checked here.
0993       bool useFirstTrackState = true;
0994       switch (targetSurfaceStrategy) {
0995         case KalmanFitterTargetSurfaceStrategy::first:
0996           useFirstTrackState = true;
0997           break;
0998         case KalmanFitterTargetSurfaceStrategy::last:
0999           useFirstTrackState = false;
1000           break;
1001         case KalmanFitterTargetSurfaceStrategy::firstOrLast:
1002           useFirstTrackState = std::abs(firstIntersection.pathLength()) <=
1003                                std::abs(lastIntersection.pathLength());
1004           break;
1005         default:
1006           ACTS_ERROR("Unknown target surface strategy");
1007           return KalmanFitterError::SmoothFailed;
1008       }
1009       bool reverseDirection = false;
1010       if (useFirstTrackState) {
1011         stepper.resetState(state.stepping, firstCreatedState.smoothed(),
1012                            firstCreatedState.smoothedCovariance(),
1013                            firstCreatedState.referenceSurface(),
1014                            state.options.maxStepSize);
1015         reverseDirection = firstIntersection.pathLength() < 0;
1016       } else {
1017         stepper.resetState(state.stepping, lastCreatedMeasurement.smoothed(),
1018                            lastCreatedMeasurement.smoothedCovariance(),
1019                            lastCreatedMeasurement.referenceSurface(),
1020                            state.options.maxStepSize);
1021         reverseDirection = lastIntersection.pathLength() < 0;
1022       }
1023       // Reverse the navigation direction if necessary
1024       if (reverseDirection) {
1025         ACTS_VERBOSE(
1026             "Reverse navigation direction after smoothing for reaching the "
1027             "target surface");
1028         state.options.direction = state.options.direction.invert();
1029       }
1030       const auto& surface = useFirstTrackState
1031                                 ? firstCreatedState.referenceSurface()
1032                                 : lastCreatedMeasurement.referenceSurface();
1033 
1034       ACTS_VERBOSE(
1035           "Smoothing successful, updating stepping state to smoothed "
1036           "parameters at surface "
1037           << surface.geometryId() << ". Prepared to reach the target surface.");
1038 
1039       // Reset the navigation state to enable propagation towards the target
1040       // surface
1041       // Set targetSurface to nullptr as it is handled manually in the actor
1042       state.navigation = navigator.makeState(&surface, nullptr);
1043       navigator.initialize(state, stepper);
1044 
1045       return Result<void>::success();
1046     }
1047   };
1048 
1049   template <typename parameters_t>
1050   class Aborter {
1051    public:
1052     /// Broadcast the action type
1053     using action_type = Actor<parameters_t>;
1054 
1055     template <typename propagator_state_t, typename stepper_t,
1056               typename navigator_t>
1057     bool operator()(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
1058                     const navigator_t& /*navigator*/,
1059                     const typename action_type::result_type& result,
1060                     const Logger& /*logger*/) const {
1061       if (!result.result.ok() || result.finished) {
1062         return true;
1063       }
1064       return false;
1065     }
1066   };
1067 
1068  public:
1069   /// Fit implementation of the forward filter, calls the
1070   /// the filter and smoother/reversed filter
1071   ///
1072   /// @tparam source_link_iterator_t Iterator type used to pass source links
1073   /// @tparam start_parameters_t Type of the initial parameters
1074   /// @tparam parameters_t Type of parameters used for local parameters
1075   /// @tparam track_container_t Type of the track container backend
1076   /// @tparam holder_t Type defining track container backend ownership
1077   ///
1078   /// @param it Begin iterator for the fittable uncalibrated measurements
1079   /// @param end End iterator for the fittable uncalibrated measurements
1080   /// @param sParameters The initial track parameters
1081   /// @param kfOptions KalmanOptions steering the fit
1082   /// @param trackContainer Input track container storage to append into
1083   /// @note The input measurements are given in the form of @c SourceLink s.
1084   /// It's the calibrators job to turn them into calibrated measurements used in
1085   /// the fit.
1086   ///
1087   /// @return the output as an output track
1088   template <typename source_link_iterator_t, typename start_parameters_t,
1089             typename parameters_t = BoundTrackParameters,
1090             typename track_container_t, template <typename> class holder_t,
1091             bool _isdn = isDirectNavigator>
1092   auto fit(source_link_iterator_t it, source_link_iterator_t end,
1093            const start_parameters_t& sParameters,
1094            const KalmanFitterOptions<traj_t>& kfOptions,
1095            TrackContainer<track_container_t, traj_t, holder_t>& trackContainer)
1096       const -> std::enable_if_t<
1097           !_isdn, Result<typename TrackContainer<track_container_t, traj_t,
1098                                                  holder_t>::TrackProxy>> {
1099     // To be able to find measurements later, we put them into a map
1100     // We need to copy input SourceLinks anyway, so the map can own them.
1101     ACTS_VERBOSE("Preparing " << std::distance(it, end)
1102                               << " input measurements");
1103     std::map<GeometryIdentifier, SourceLink> inputMeasurements;
1104     // for (const auto& sl : sourcelinks) {
1105     for (; it != end; ++it) {
1106       SourceLink sl = *it;
1107       const Surface* surface = kfOptions.extensions.surfaceAccessor(sl);
1108       // @TODO: This can probably change over to surface pointers as keys
1109       auto geoId = surface->geometryId();
1110       inputMeasurements.emplace(geoId, std::move(sl));
1111     }
1112 
1113     // Create the ActionList and AbortList
1114     using KalmanAborter = Aborter<parameters_t>;
1115     using KalmanActor = Actor<parameters_t>;
1116 
1117     using KalmanResult = typename KalmanActor::result_type;
1118     using Actors = ActionList<KalmanActor>;
1119     using Aborters = AbortList<KalmanAborter>;
1120 
1121     // Create relevant options for the propagation options
1122     PropagatorOptions<Actors, Aborters> kalmanOptions(
1123         kfOptions.geoContext, kfOptions.magFieldContext);
1124 
1125     // Set the trivial propagator options
1126     kalmanOptions.setPlainOptions(kfOptions.propagatorPlainOptions);
1127 
1128     // Catch the actor and set the measurements
1129     auto& kalmanActor = kalmanOptions.actionList.template get<KalmanActor>();
1130     kalmanActor.inputMeasurements = &inputMeasurements;
1131     kalmanActor.targetReached.surface = kfOptions.referenceSurface;
1132     kalmanActor.targetSurfaceStrategy = kfOptions.referenceSurfaceStrategy;
1133     kalmanActor.multipleScattering = kfOptions.multipleScattering;
1134     kalmanActor.energyLoss = kfOptions.energyLoss;
1135     kalmanActor.reversedFiltering = kfOptions.reversedFiltering;
1136     kalmanActor.reversedFilteringCovarianceScaling =
1137         kfOptions.reversedFilteringCovarianceScaling;
1138     kalmanActor.freeToBoundCorrection = kfOptions.freeToBoundCorrection;
1139     kalmanActor.calibrationContext = &kfOptions.calibrationContext.get();
1140     kalmanActor.extensions = kfOptions.extensions;
1141     kalmanActor.actorLogger = m_actorLogger.get();
1142 
1143     return fit_impl<start_parameters_t, Actors, Aborters, KalmanResult,
1144                     track_container_t, holder_t>(sParameters, kalmanOptions,
1145                                                  trackContainer);
1146   }
1147 
1148   /// Fit implementation of the forward filter, calls the
1149   /// the filter and smoother/reversed filter
1150   ///
1151   /// @tparam source_link_iterator_t Iterator type used to pass source links
1152   /// @tparam start_parameters_t Type of the initial parameters
1153   /// @tparam parameters_t Type of parameters used for local parameters
1154   /// @tparam track_container_t Type of the track container backend
1155   /// @tparam holder_t Type defining track container backend ownership
1156   ///
1157   /// @param it Begin iterator for the fittable uncalibrated measurements
1158   /// @param end End iterator for the fittable uncalibrated measurements
1159   /// @param sParameters The initial track parameters
1160   /// @param kfOptions KalmanOptions steering the fit
1161   /// @param sSequence surface sequence used to initialize a DirectNavigator
1162   /// @param trackContainer Input track container storage to append into
1163   /// @note The input measurements are given in the form of @c SourceLinks.
1164   /// It's
1165   /// @c calibrator_t's job to turn them into calibrated measurements used in
1166   /// the fit.
1167   ///
1168   /// @return the output as an output track
1169   template <typename source_link_iterator_t, typename start_parameters_t,
1170             typename parameters_t = BoundTrackParameters,
1171             typename track_container_t, template <typename> class holder_t,
1172             bool _isdn = isDirectNavigator>
1173   auto fit(source_link_iterator_t it, source_link_iterator_t end,
1174            const start_parameters_t& sParameters,
1175            const KalmanFitterOptions<traj_t>& kfOptions,
1176            const std::vector<const Surface*>& sSequence,
1177            TrackContainer<track_container_t, traj_t, holder_t>& trackContainer)
1178       const -> std::enable_if_t<
1179           _isdn, Result<typename TrackContainer<track_container_t, traj_t,
1180                                                 holder_t>::TrackProxy>> {
1181     // To be able to find measurements later, we put them into a map
1182     // We need to copy input SourceLinks anyway, so the map can own them.
1183     ACTS_VERBOSE("Preparing " << std::distance(it, end)
1184                               << " input measurements");
1185     std::map<GeometryIdentifier, SourceLink> inputMeasurements;
1186     for (; it != end; ++it) {
1187       SourceLink sl = *it;
1188       const Surface* surface = kfOptions.extensions.surfaceAccessor(sl);
1189       // @TODO: This can probably change over to surface pointers as keys
1190       auto geoId = surface->geometryId();
1191       inputMeasurements.emplace(geoId, std::move(sl));
1192     }
1193 
1194     // Create the ActionList and AbortList
1195     using KalmanAborter = Aborter<parameters_t>;
1196     using KalmanActor = Actor<parameters_t>;
1197 
1198     using KalmanResult = typename KalmanActor::result_type;
1199     using Actors = ActionList<DirectNavigator::Initializer, KalmanActor>;
1200     using Aborters = AbortList<KalmanAborter>;
1201 
1202     // Create relevant options for the propagation options
1203     PropagatorOptions<Actors, Aborters> kalmanOptions(
1204         kfOptions.geoContext, kfOptions.magFieldContext);
1205 
1206     // Set the trivial propagator options
1207     kalmanOptions.setPlainOptions(kfOptions.propagatorPlainOptions);
1208 
1209     // Catch the actor and set the measurements
1210     auto& kalmanActor = kalmanOptions.actionList.template get<KalmanActor>();
1211     kalmanActor.inputMeasurements = &inputMeasurements;
1212     kalmanActor.targetReached.surface = kfOptions.referenceSurface;
1213     kalmanActor.targetSurfaceStrategy = kfOptions.referenceSurfaceStrategy;
1214     kalmanActor.multipleScattering = kfOptions.multipleScattering;
1215     kalmanActor.energyLoss = kfOptions.energyLoss;
1216     kalmanActor.reversedFiltering = kfOptions.reversedFiltering;
1217     kalmanActor.reversedFilteringCovarianceScaling =
1218         kfOptions.reversedFilteringCovarianceScaling;
1219     kalmanActor.extensions = kfOptions.extensions;
1220     kalmanActor.actorLogger = m_actorLogger.get();
1221 
1222     // Set the surface sequence
1223     auto& dInitializer =
1224         kalmanOptions.actionList.template get<DirectNavigator::Initializer>();
1225     dInitializer.navSurfaces = sSequence;
1226 
1227     return fit_impl<start_parameters_t, Actors, Aborters, KalmanResult,
1228                     track_container_t, holder_t>(sParameters, kalmanOptions,
1229                                                  trackContainer);
1230   }
1231 
1232  private:
1233   /// Common fit implementation
1234   ///
1235   /// @tparam start_parameters_t Type of the initial parameters
1236   /// @tparam actor_list_t Type of the actor list
1237   /// @tparam aborter_list_t Type of the abort list
1238   /// @tparam kalman_result_t Type of the KF result
1239   /// @tparam track_container_t Type of the track container backend
1240   /// @tparam holder_t Type defining track container backend ownership
1241   ///
1242   /// @param sParameters The initial track parameters
1243   /// @param kalmanOptions The Kalman Options
1244   /// @param trackContainer Input track container storage to append into
1245   ///
1246   /// @return the output as an output track
1247   template <typename start_parameters_t, typename actor_list_t,
1248             typename aborter_list_t, typename kalman_result_t,
1249             typename track_container_t, template <typename> class holder_t>
1250   auto fit_impl(
1251       const start_parameters_t& sParameters,
1252       const PropagatorOptions<actor_list_t, aborter_list_t>& kalmanOptions,
1253       TrackContainer<track_container_t, traj_t, holder_t>& trackContainer) const
1254       -> Result<typename TrackContainer<track_container_t, traj_t,
1255                                         holder_t>::TrackProxy> {
1256     auto propagatorState =
1257         m_propagator.template makeState(sParameters, kalmanOptions);
1258 
1259     auto& kalmanResult =
1260         propagatorState.template get<KalmanFitterResult<traj_t>>();
1261     kalmanResult.fittedStates = &trackContainer.trackStateContainer();
1262 
1263     // Run the fitter
1264     auto result = m_propagator.template propagate(propagatorState);
1265 
1266     if (!result.ok()) {
1267       ACTS_ERROR("Propagation failed: " << result.error());
1268       return result.error();
1269     }
1270 
1271     /// It could happen that the fit ends in zero measurement states.
1272     /// The result gets meaningless so such case is regarded as fit failure.
1273     if (kalmanResult.result.ok() && !kalmanResult.measurementStates) {
1274       kalmanResult.result = Result<void>(KalmanFitterError::NoMeasurementFound);
1275     }
1276 
1277     if (!kalmanResult.result.ok()) {
1278       ACTS_ERROR("KalmanFilter failed: "
1279                  << kalmanResult.result.error() << ", "
1280                  << kalmanResult.result.error().message());
1281       return kalmanResult.result.error();
1282     }
1283 
1284     auto track = trackContainer.makeTrack();
1285     track.tipIndex() = kalmanResult.lastMeasurementIndex;
1286     if (kalmanResult.fittedParameters) {
1287       const auto& params = kalmanResult.fittedParameters.value();
1288       track.parameters() = params.parameters();
1289       track.covariance() = params.covariance().value();
1290       track.setReferenceSurface(params.referenceSurface().getSharedPtr());
1291     }
1292 
1293     calculateTrackQuantities(track);
1294 
1295     if (trackContainer.hasColumn(hashString("smoothed"))) {
1296       track.template component<bool, hashString("smoothed")>() =
1297           kalmanResult.smoothed;
1298     }
1299 
1300     if (trackContainer.hasColumn(hashString("reversed"))) {
1301       track.template component<bool, hashString("reversed")>() =
1302           kalmanResult.reversed;
1303     }
1304 
1305     // Return the converted Track
1306     return track;
1307   }
1308 };
1309 
1310 }  // namespace Acts