Back to home page

EIC code displayed by LXR

 
 

    


File indexing completed on 2025-09-14 08:12:09

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