Back to home page

EIC code displayed by LXR

 
 

    


File indexing completed on 2025-07-13 08:05:02

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2016-2024 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/Definitions/Algebra.hpp"
0015 #include "Acts/Definitions/Common.hpp"
0016 #include "Acts/EventData/MeasurementHelpers.hpp"
0017 #include "Acts/EventData/MultiTrajectory.hpp"
0018 #include "Acts/EventData/MultiTrajectoryHelpers.hpp"
0019 #include "Acts/EventData/ProxyAccessor.hpp"
0020 #include "Acts/EventData/TrackContainer.hpp"
0021 #include "Acts/EventData/TrackParameters.hpp"
0022 #include "Acts/EventData/TrackStatePropMask.hpp"
0023 #include "Acts/EventData/Types.hpp"
0024 #include "Acts/Geometry/GeometryContext.hpp"
0025 #include "Acts/MagneticField/MagneticFieldContext.hpp"
0026 #include "Acts/Material/MaterialSlab.hpp"
0027 #include "Acts/Propagator/AbortList.hpp"
0028 #include "Acts/Propagator/ActionList.hpp"
0029 #include "Acts/Propagator/ConstrainedStep.hpp"
0030 #include "Acts/Propagator/Propagator.hpp"
0031 #include "Acts/Propagator/StandardAborters.hpp"
0032 #include "Acts/Propagator/detail/LoopProtection.hpp"
0033 #include "Acts/Propagator/detail/PointwiseMaterialInteraction.hpp"
0034 #include "Acts/Surfaces/BoundaryTolerance.hpp"
0035 #include "Acts/TrackFinding/CombinatorialKalmanFilterError.hpp"
0036 #include "Acts/TrackFitting/KalmanFitter.hpp"
0037 #include "Acts/TrackFitting/detail/VoidFitterComponents.hpp"
0038 #include "Acts/Utilities/CalibrationContext.hpp"
0039 #include "Acts/Utilities/HashedString.hpp"
0040 #include "Acts/Utilities/Logger.hpp"
0041 #include "Acts/Utilities/Result.hpp"
0042 #include "Acts/Utilities/TrackHelpers.hpp"
0043 #include "Acts/Utilities/Zip.hpp"
0044 
0045 #include <functional>
0046 #include <limits>
0047 #include <memory>
0048 #include <string_view>
0049 #include <type_traits>
0050 #include <unordered_map>
0051 
0052 namespace Acts {
0053 
0054 /// Return type of the `BranchStopper` delegate for the
0055 /// CombinatorialKalmanFilter
0056 enum class CombinatorialKalmanFilterBranchStopperResult {
0057   Continue,
0058   StopAndDrop,
0059   StopAndKeep,
0060 };
0061 
0062 /// Extension struct which holds the delegates to customize the CKF behavior
0063 template <typename track_container_t>
0064 struct CombinatorialKalmanFilterExtensions {
0065   using traj_t = typename track_container_t::TrackStateContainerBackend;
0066   using candidate_container_t =
0067       typename std::vector<typename track_container_t::TrackStateProxy>;
0068   using TrackProxy = typename track_container_t::TrackProxy;
0069   using TrackStateProxy = typename track_container_t::TrackStateProxy;
0070 
0071   using BranchStopperResult = CombinatorialKalmanFilterBranchStopperResult;
0072 
0073   using Calibrator = typename KalmanFitterExtensions<traj_t>::Calibrator;
0074   using Updater = typename KalmanFitterExtensions<traj_t>::Updater;
0075   using MeasurementSelector =
0076       Delegate<Result<std::pair<typename candidate_container_t::iterator,
0077                                 typename candidate_container_t::iterator>>(
0078           candidate_container_t& trackStates, bool&, const Logger&)>;
0079   using BranchStopper =
0080       Delegate<BranchStopperResult(const TrackProxy&, const TrackStateProxy&)>;
0081 
0082   /// The Calibrator is a dedicated calibration algorithm that allows to
0083   /// calibrate measurements using track information, this could be e.g. sagging
0084   /// for wires, module deformations, etc.
0085   Calibrator calibrator{
0086       DelegateFuncTag<detail::voidFitterCalibrator<traj_t>>{}};
0087 
0088   /// The updater incorporates measurement information into the track parameters
0089   Updater updater{DelegateFuncTag<detail::voidFitterUpdater<traj_t>>{}};
0090 
0091   /// The measurement selector is called during the filtering by the Actor.
0092   MeasurementSelector measurementSelector{
0093       DelegateFuncTag<voidMeasurementSelector>{}};
0094 
0095   /// The branch stopper is called during the filtering by the Actor.
0096   BranchStopper branchStopper{DelegateFuncTag<voidBranchStopper>{}};
0097 
0098  private:
0099   /// Default measurement selector which will return all measurements
0100   /// @param candidates Measurement track state candidates
0101   static Result<std::pair<typename std::vector<TrackStateProxy>::iterator,
0102                           typename std::vector<TrackStateProxy>::iterator>>
0103   voidMeasurementSelector(typename std::vector<TrackStateProxy>& candidates,
0104                           bool& /*isOutlier*/, const Logger& /*logger*/) {
0105     return std::pair{candidates.begin(), candidates.end()};
0106   };
0107 
0108   /// Default branch stopper which will never stop
0109   /// @return false
0110   static BranchStopperResult voidBranchStopper(
0111       const TrackProxy& /*track*/, const TrackStateProxy& /*trackState*/) {
0112     return BranchStopperResult::Continue;
0113   }
0114 };
0115 
0116 /// Delegate type that retrieves a range of source links to for a given surface
0117 /// to be processed by the CKF
0118 template <typename source_link_iterator_t>
0119 using SourceLinkAccessorDelegate =
0120     Delegate<std::pair<source_link_iterator_t, source_link_iterator_t>(
0121         const Surface&)>;
0122 
0123 /// expected max number of track states that are expected to be added by
0124 /// stateCandidateCreator
0125 /// @note if the number of states exceeds this number dynamic memory allocation will occur.
0126 ///       the number is chosen to yield a container size of 64 bytes.
0127 static constexpr std::size_t s_maxBranchesPerSurface = 10;
0128 
0129 namespace CkfTypes {
0130 
0131 template <typename T>
0132 using BranchVector = boost::container::small_vector<T, s_maxBranchesPerSurface>;
0133 
0134 }  // namespace CkfTypes
0135 
0136 /// Combined options for the combinatorial Kalman filter.
0137 ///
0138 /// @tparam source_link_iterator_t Type of the source link iterator
0139 /// @tparam track_container_t Type of the track container
0140 template <typename source_link_iterator_t, typename track_container_t>
0141 struct CombinatorialKalmanFilterOptions {
0142   using SourceLinkIterator = source_link_iterator_t;
0143   using SourceLinkAccessor = SourceLinkAccessorDelegate<source_link_iterator_t>;
0144 
0145   using TrackStateContainerBackend =
0146       typename track_container_t::TrackStateContainerBackend;
0147   using TrackStateProxy = typename track_container_t::TrackStateProxy;
0148 
0149   /// PropagatorOptions with context
0150   ///
0151   /// @param gctx The geometry context for this track finding/fitting
0152   /// @param mctx The magnetic context for this track finding/fitting
0153   /// @param cctx The calibration context for this track finding/fitting
0154   /// @param accessor_ The source link accessor
0155   /// @param extensions_ The extension struct
0156   /// @param pOptions The plain propagator options
0157   /// @param mScattering Whether to include multiple scattering
0158   /// @param eLoss Whether to include energy loss
0159   CombinatorialKalmanFilterOptions(
0160       const GeometryContext& gctx, const MagneticFieldContext& mctx,
0161       std::reference_wrapper<const CalibrationContext> cctx,
0162       SourceLinkAccessor accessor_,
0163       CombinatorialKalmanFilterExtensions<track_container_t> extensions_,
0164       const PropagatorPlainOptions& pOptions, bool mScattering = true,
0165       bool eLoss = true)
0166       : geoContext(gctx),
0167         magFieldContext(mctx),
0168         calibrationContext(cctx),
0169         sourcelinkAccessor(std::move(accessor_)),
0170         extensions(extensions_),
0171         propagatorPlainOptions(pOptions),
0172         multipleScattering(mScattering),
0173         energyLoss(eLoss) {}
0174 
0175   /// Contexts are required and the options must not be default-constructible.
0176   CombinatorialKalmanFilterOptions() = delete;
0177 
0178   /// Context object for the geometry
0179   std::reference_wrapper<const GeometryContext> geoContext;
0180   /// Context object for the magnetic field
0181   std::reference_wrapper<const MagneticFieldContext> magFieldContext;
0182   /// context object for the calibration
0183   std::reference_wrapper<const CalibrationContext> calibrationContext;
0184 
0185   /// The source link accessor
0186   SourceLinkAccessor sourcelinkAccessor;
0187 
0188   /// The filter extensions
0189   CombinatorialKalmanFilterExtensions<track_container_t> extensions;
0190 
0191   /// The trivial propagator options
0192   PropagatorPlainOptions propagatorPlainOptions;
0193 
0194   /// The target surface
0195   /// @note This is useful if the filtering should be terminated at a
0196   ///       certain surface
0197   const Surface* targetSurface = nullptr;
0198 
0199   using BoundState = std::tuple<BoundTrackParameters, BoundMatrix, double>;
0200 
0201   /// Delegate definition to create track states for selected measurements
0202   ///
0203   /// @note expected to iterator over the given sourcelink range,
0204   ///       select measurements, and create track states for
0205   ///       which new tips are to be created, more over the outlier
0206   ///       flag should be set for states that are outlier.
0207   ///
0208   /// @param geoContext The current geometry context
0209   /// @param calibrationContext pointer to the current calibration context
0210   /// @param surface the surface at which new track states are to be created
0211   /// @param boundState the current bound state of the trajectory
0212   /// @param slBegin Begin iterator for sourcelinks
0213   /// @param slEnd End iterator for sourcelinks
0214   /// @param prevTip Index pointing at previous trajectory state (i.e. tip)
0215   /// @param bufferTrajectory a temporary trajectory which can be used to create temporary track states
0216   /// @param trackStateCandidates a temporary buffer that can be used to collect track states
0217   /// @param trajectory the trajectory to which the new states are to be added
0218   /// @param logger a logger for messages
0219   using TrackStateCandidateCreator =
0220       Delegate<Result<CkfTypes::BranchVector<TrackIndexType>>(
0221           const GeometryContext& geoContext,
0222           const CalibrationContext& calibrationContext, const Surface& surface,
0223           const BoundState& boundState, source_link_iterator_t slBegin,
0224           source_link_iterator_t slEnd, TrackIndexType prevTip,
0225           TrackStateContainerBackend& bufferTrajectory,
0226           std::vector<TrackStateProxy>& trackStateCandidates,
0227           TrackStateContainerBackend& trajectory, const Logger& logger)>;
0228 
0229   /// The delegate to create new track states.
0230   TrackStateCandidateCreator trackStateCandidateCreator;
0231 
0232   /// Whether to consider multiple scattering.
0233   bool multipleScattering = true;
0234 
0235   /// Whether to consider energy loss.
0236   bool energyLoss = true;
0237 
0238   /// Skip the pre propagation call. This effectively skips the first surface
0239   /// @note This is useful if the first surface should not be considered in a second reverse pass
0240   bool skipPrePropagationUpdate = false;
0241 };
0242 
0243 template <typename track_container_t>
0244 struct CombinatorialKalmanFilterResult {
0245   using TrackStateContainerBackend =
0246       typename track_container_t::TrackStateContainerBackend;
0247   using TrackProxy = typename track_container_t::TrackProxy;
0248   using TrackStateProxy = typename track_container_t::TrackStateProxy;
0249 
0250   /// The track container to store the found tracks
0251   track_container_t* tracks{nullptr};
0252 
0253   /// Fitted states that the actor has handled.
0254   TrackStateContainerBackend* trackStates{nullptr};
0255 
0256   /// Indices into `tracks` which mark active branches
0257   std::vector<TrackProxy> activeBranches;
0258 
0259   /// Indices into `tracks` which mark active branches
0260   std::vector<TrackProxy> collectedTracks;
0261 
0262   /// Track state candidates buffer
0263   std::vector<TrackStateProxy> trackStateCandidates;
0264 
0265   /// Indicator if track finding has been done
0266   bool finished = false;
0267 
0268   /// Last encountered error
0269   Result<void> lastError{Result<void>::success()};
0270 
0271   /// Path limit aborter
0272   PathLimitReached pathLimitReached;
0273 };
0274 
0275 /// Combinatorial Kalman filter to find tracks.
0276 ///
0277 /// @tparam propagator_t Type of the propagator
0278 ///
0279 /// The CombinatorialKalmanFilter contains an Actor and a Sequencer sub-class.
0280 /// The Sequencer has to be part of the Navigator of the Propagator in order to
0281 /// initialize and provide the measurement surfaces.
0282 ///
0283 /// The Actor is part of the Propagation call and does the Kalman update.
0284 /// Updater and Calibrator are given to the Actor for further use:
0285 /// - The Updater is the implemented kalman updater formalism, it
0286 ///   runs via a visitor pattern through the measurements.
0287 ///
0288 /// Measurements are not required to be ordered for the
0289 /// CombinatorialKalmanFilter, measurement ordering needs to be figured out by
0290 /// the navigation of the propagator.
0291 ///
0292 /// The void components are provided mainly for unit testing.
0293 ///
0294 template <typename propagator_t, typename track_container_t>
0295 class CombinatorialKalmanFilter {
0296  public:
0297   /// Default constructor is deleted
0298   CombinatorialKalmanFilter() = delete;
0299   /// Constructor from arguments
0300   CombinatorialKalmanFilter(propagator_t pPropagator,
0301                             std::unique_ptr<const Logger> _logger =
0302                                 getDefaultLogger("CKF", Logging::INFO))
0303       : m_propagator(std::move(pPropagator)),
0304         m_logger(std::move(_logger)),
0305         m_actorLogger{m_logger->cloneWithSuffix("Actor")},
0306         m_updaterLogger{m_logger->cloneWithSuffix("Updater")} {}
0307 
0308  private:
0309   using BoundState = std::tuple<BoundTrackParameters, BoundMatrix, double>;
0310   using TrackStateContainerBackend =
0311       typename track_container_t::TrackStateContainerBackend;
0312   using TrackProxy = typename track_container_t::TrackProxy;
0313   using TrackStateProxy = typename track_container_t::TrackStateProxy;
0314 
0315   /// The propagator for the transport and material update
0316   propagator_t m_propagator;
0317 
0318   std::unique_ptr<const Logger> m_logger;
0319   std::shared_ptr<const Logger> m_actorLogger;
0320   std::shared_ptr<const Logger> m_updaterLogger;
0321 
0322   const Logger& logger() const { return *m_logger; }
0323 
0324   struct DefaultTrackStateCreator {
0325     typename CombinatorialKalmanFilterExtensions<track_container_t>::Calibrator
0326         calibrator;
0327     typename CombinatorialKalmanFilterExtensions<
0328         track_container_t>::MeasurementSelector measurementSelector;
0329 
0330     /// Create track states for selected measurements given by the source links
0331     ///
0332     /// @param gctx The current geometry context
0333     /// @param calibrationContext pointer to the current calibration context
0334     /// @param surface the surface the sourceLinks are associated to
0335     /// @param boundState Bound state from the propagation on this surface
0336     /// @param slBegin Begin iterator for sourcelinks
0337     /// @param slEnd End iterator for sourcelinks
0338     /// @param prevTip Index pointing at previous trajectory state (i.e. tip)
0339     /// @param bufferTrajectory a buffer for temporary candidate track states
0340     /// @param trackStateCandidates a buffer for temporary track state proxies for candidates
0341     /// @param trajectory the trajectory to which new track states for selected measurements will be added
0342     /// @param logger the logger for messages.
0343     template <typename source_link_iterator_t>
0344     Result<CkfTypes::BranchVector<TrackIndexType>> createSourceLinkTrackStates(
0345         const GeometryContext& gctx,
0346         const CalibrationContext& calibrationContext,
0347         [[maybe_unused]] const Surface& surface, const BoundState& boundState,
0348         source_link_iterator_t slBegin, source_link_iterator_t slEnd,
0349         TrackIndexType prevTip, TrackStateContainerBackend& bufferTrajectory,
0350         std::vector<TrackStateProxy>& trackStateCandidates,
0351         TrackStateContainerBackend& trajectory, const Logger& logger) const {
0352       using PM = TrackStatePropMask;
0353 
0354       using ResultTrackStateList =
0355           Acts::Result<CkfTypes::BranchVector<TrackIndexType>>;
0356       ResultTrackStateList resultTrackStateList{
0357           CkfTypes::BranchVector<TrackIndexType>()};
0358       const auto& [boundParams, jacobian, pathLength] = boundState;
0359 
0360       trackStateCandidates.clear();
0361       if constexpr (std::is_same_v<
0362                         typename std::iterator_traits<
0363                             source_link_iterator_t>::iterator_category,
0364                         std::random_access_iterator_tag>) {
0365         trackStateCandidates.reserve(std::distance(slBegin, slEnd));
0366       }
0367 
0368       // Calibrate all the source links on the surface since the selection has
0369       // to be done based on calibrated measurement
0370       for (auto it = slBegin; it != slEnd; ++it) {
0371         // get the source link
0372         const auto sourceLink = *it;
0373 
0374         // prepare the track state
0375         PM mask = PM::Predicted | PM::Jacobian | PM::Calibrated;
0376         if (it != slBegin) {
0377           // not the first TrackState, only need uncalibrated and calibrated
0378           mask = PM::Calibrated;
0379         }
0380 
0381         ACTS_VERBOSE("Create temp track state with mask: " << mask);
0382         // CAREFUL! This trackstate has a previous index that is not in this
0383         // MultiTrajectory Visiting brackwards from this track state will
0384         // fail!
0385         auto ts = bufferTrajectory.makeTrackState(mask, prevTip);
0386 
0387         if (it == slBegin) {
0388           // only set these for first
0389           ts.predicted() = boundParams.parameters();
0390           if (boundParams.covariance()) {
0391             ts.predictedCovariance() = *boundParams.covariance();
0392           }
0393           ts.jacobian() = jacobian;
0394         } else {
0395           // subsequent track states can reuse
0396           auto& first = trackStateCandidates.front();
0397           ts.shareFrom(first, PM::Predicted);
0398           ts.shareFrom(first, PM::Jacobian);
0399         }
0400 
0401         ts.pathLength() = pathLength;
0402         ts.setReferenceSurface(boundParams.referenceSurface().getSharedPtr());
0403 
0404         // now calibrate the track state
0405         calibrator(gctx, calibrationContext, sourceLink, ts);
0406 
0407         trackStateCandidates.push_back(ts);
0408       }
0409 
0410       bool isOutlier = false;
0411       Result<std::pair<typename std::vector<TrackStateProxy>::iterator,
0412                        typename std::vector<TrackStateProxy>::iterator>>
0413           selectorResult =
0414               measurementSelector(trackStateCandidates, isOutlier, logger);
0415       if (!selectorResult.ok()) {
0416         ACTS_ERROR("Selection of calibrated measurements failed: "
0417                    << selectorResult.error());
0418         resultTrackStateList =
0419             ResultTrackStateList::failure(selectorResult.error());
0420       } else {
0421         auto selectedTrackStateRange = *selectorResult;
0422         resultTrackStateList = processSelectedTrackStates(
0423             selectedTrackStateRange.first, selectedTrackStateRange.second,
0424             trajectory, isOutlier, logger);
0425       }
0426 
0427       return resultTrackStateList;
0428     }
0429 
0430     /// Create track states for the given trajectory from candidate track states
0431     ///
0432     /// @param begin begin iterator of the list of candidate track states
0433     /// @param end end iterator of the list of candidate track states
0434     /// @param trackStates the trajectory to which the new track states are added
0435     /// @param isOutlier true if the candidate(s) is(are) an outlier(s).
0436     /// @param logger the logger for messages
0437     Result<CkfTypes::BranchVector<TrackIndexType>> processSelectedTrackStates(
0438         typename std::vector<TrackStateProxy>::const_iterator begin,
0439         typename std::vector<TrackStateProxy>::const_iterator end,
0440         TrackStateContainerBackend& trackStates, bool isOutlier,
0441         const Logger& logger) const {
0442       using PM = TrackStatePropMask;
0443 
0444       using ResultTrackStateList =
0445           Acts::Result<CkfTypes::BranchVector<TrackIndexType>>;
0446       ResultTrackStateList resultTrackStateList{
0447           CkfTypes::BranchVector<TrackIndexType>()};
0448       CkfTypes::BranchVector<TrackIndexType>& trackStateList =
0449           *resultTrackStateList;
0450       trackStateList.reserve(end - begin);
0451 
0452       std::optional<TrackStateProxy> firstTrackState{std::nullopt};
0453       for (auto it = begin; it != end; ++it) {
0454         auto& candidateTrackState = *it;
0455 
0456         PM mask = PM::Predicted | PM::Filtered | PM::Jacobian | PM::Calibrated;
0457         if (it != begin) {
0458           // subsequent track states don't need storage for these as they will
0459           // be shared
0460           mask &= ~PM::Predicted & ~PM::Jacobian;
0461         }
0462         if (isOutlier) {
0463           // outlier won't have separate filtered parameters
0464           mask &= ~PM::Filtered;
0465         }
0466 
0467         // copy this trackstate into fitted states MultiTrajectory
0468         auto trackState =
0469             trackStates.makeTrackState(mask, candidateTrackState.previous());
0470         ACTS_VERBOSE("Create SourceLink output track state #"
0471                      << trackState.index() << " with mask: " << mask);
0472 
0473         if (it != begin) {
0474           // assign indices pointing to first track state
0475           trackState.shareFrom(*firstTrackState, PM::Predicted);
0476           trackState.shareFrom(*firstTrackState, PM::Jacobian);
0477         } else {
0478           firstTrackState = trackState;
0479         }
0480 
0481         // either copy ALL or everything except for predicted and jacobian
0482         trackState.allocateCalibrated(candidateTrackState.calibratedSize());
0483         trackState.copyFrom(candidateTrackState, mask, false);
0484 
0485         auto typeFlags = trackState.typeFlags();
0486         typeFlags.set(TrackStateFlag::ParameterFlag);
0487         typeFlags.set(TrackStateFlag::MeasurementFlag);
0488         if (trackState.referenceSurface().surfaceMaterial() != nullptr) {
0489           typeFlags.set(TrackStateFlag::MaterialFlag);
0490         }
0491         if (isOutlier) {
0492           // propagate information that this is an outlier state
0493           ACTS_VERBOSE(
0494               "Creating outlier track state with tip = " << trackState.index());
0495           typeFlags.set(TrackStateFlag::OutlierFlag);
0496         }
0497 
0498         trackStateList.push_back(trackState.index());
0499       }
0500       return resultTrackStateList;
0501     }
0502   };
0503 
0504   /// @brief Propagator Actor plugin for the CombinatorialKalmanFilter
0505   ///
0506   /// @tparam source_link_accessor_t The type of source link accessor
0507   /// @tparam parameters_t The type of parameters used for "local" parameters.
0508   ///
0509   /// The CombinatorialKalmanFilter Actor does not rely on the measurements to
0510   /// be sorted along the track.
0511   template <typename source_link_accessor_t, typename parameters_t>
0512   class Actor {
0513    public:
0514     using BoundState = std::tuple<parameters_t, BoundMatrix, double>;
0515     using CurvilinearState =
0516         std::tuple<CurvilinearTrackParameters, BoundMatrix, double>;
0517     /// Broadcast the result_type
0518     using result_type = CombinatorialKalmanFilterResult<track_container_t>;
0519 
0520     using BranchStopperResult = CombinatorialKalmanFilterBranchStopperResult;
0521 
0522     /// The target surface aborter
0523     SurfaceReached targetReached{std::numeric_limits<double>::lowest()};
0524 
0525     /// Whether to consider multiple scattering.
0526     bool multipleScattering = true;
0527 
0528     /// Whether to consider energy loss.
0529     bool energyLoss = true;
0530 
0531     /// Skip the pre propagation call. This effectively skips the first surface
0532     bool skipPrePropagationUpdate = false;
0533 
0534     /// Calibration context for the finding run
0535     const CalibrationContext* calibrationContextPtr{nullptr};
0536 
0537     /// @brief CombinatorialKalmanFilter actor operation
0538     ///
0539     /// @tparam propagator_state_t Type of the Propagator state
0540     /// @tparam stepper_t Type of the stepper
0541     ///
0542     /// @param state is the mutable propagator state object
0543     /// @param stepper is the stepper in use
0544     /// @param navigator is the navigator in use
0545     /// @param result is the mutable result state object
0546     template <typename propagator_state_t, typename stepper_t,
0547               typename navigator_t>
0548     void operator()(propagator_state_t& state, const stepper_t& stepper,
0549                     const navigator_t& navigator, result_type& result,
0550                     const Logger& /*logger*/) const {
0551       assert(result.trackStates && "No MultiTrajectory set");
0552 
0553       if (result.finished) {
0554         return;
0555       }
0556 
0557       if (state.stage == PropagatorStage::prePropagation &&
0558           skipPrePropagationUpdate) {
0559         ACTS_VERBOSE("Skip pre-propagation update (first surface)");
0560         return;
0561       }
0562 
0563       ACTS_VERBOSE("CombinatorialKalmanFilter step");
0564 
0565       assert(!result.activeBranches.empty() && "No active branches");
0566 
0567       // Initialize path limit reached aborter
0568       if (result.pathLimitReached.internalLimit ==
0569           std::numeric_limits<double>::max()) {
0570         detail::setupLoopProtection(state, stepper, result.pathLimitReached,
0571                                     true, logger());
0572       }
0573 
0574       // Update:
0575       // - Waiting for a current surface
0576       if (auto surface = navigator.currentSurface(state.navigation);
0577           surface != nullptr) {
0578         // There are three scenarios:
0579         // 1) The surface is in the measurement map
0580         // -> Select source links
0581         // -> Perform the kalman update for selected non-outlier source links
0582         // -> Add track states in multitrajectory. Multiple states mean branch
0583         // splitting.
0584         // -> Call branch stopper to justify each branch
0585         // -> If there is non-outlier state, update stepper information
0586         // 2) The surface is not in the measurement map but with material or is
0587         // an active surface
0588         // -> Add a hole or passive material state in multitrajectory
0589         // -> Call branch stopper to justify the branch
0590         // 3) The surface is neither in the measurement map nor with material
0591         // -> Do nothing
0592         ACTS_VERBOSE("Perform filter step");
0593         auto res = filter(surface, state, stepper, navigator, result);
0594         if (!res.ok()) {
0595           ACTS_ERROR("Error in filter: " << res.error());
0596           result.lastError = res.error();
0597         }
0598       }
0599 
0600       const bool isEndOfWorldReached =
0601           endOfWorldReached(state, stepper, navigator, logger());
0602       const bool isPathLimitReached =
0603           result.pathLimitReached(state, stepper, navigator, logger());
0604       const bool isTargetReached =
0605           targetReached(state, stepper, navigator, logger());
0606       const bool allBranchesStopped = result.activeBranches.empty();
0607       if (isEndOfWorldReached || isPathLimitReached || isTargetReached ||
0608           allBranchesStopped) {
0609         if (isEndOfWorldReached) {
0610           ACTS_VERBOSE("End of world reached");
0611         } else if (isPathLimitReached) {
0612           ACTS_VERBOSE("Path limit reached");
0613         } else if (isTargetReached) {
0614           ACTS_VERBOSE("Target surface reached");
0615 
0616           // Bind the parameter to the target surface
0617           auto res = stepper.boundState(state.stepping, *targetReached.surface);
0618           if (!res.ok()) {
0619             ACTS_ERROR("Error while acquiring bound state for target surface: "
0620                        << res.error() << " " << res.error().message());
0621             result.lastError = res.error();
0622           } else {
0623             const auto& [boundParams, jacobian, pathLength] = *res;
0624             auto currentBranch = result.activeBranches.back();
0625             // Assign the fitted parameters
0626             currentBranch.parameters() = boundParams.parameters();
0627             currentBranch.covariance() = *boundParams.covariance();
0628             currentBranch.setReferenceSurface(
0629                 boundParams.referenceSurface().getSharedPtr());
0630           }
0631 
0632           stepper.releaseStepSize(state.stepping, ConstrainedStep::actor);
0633         }
0634 
0635         if (!allBranchesStopped) {
0636           // Record the active branch and remove it from the list
0637           storeLastActiveBranch(result);
0638           result.activeBranches.pop_back();
0639         } else {
0640           // This can happen if we stopped all branches in the filter step
0641           ACTS_VERBOSE("All branches stopped");
0642         }
0643 
0644         // If no more active branches, done with filtering; Otherwise, reset
0645         // propagation state to track state at next active branch
0646         if (!result.activeBranches.empty()) {
0647           ACTS_VERBOSE("Propagation jumps to branch with tip = "
0648                        << result.activeBranches.back().tipIndex());
0649           reset(state, stepper, navigator, result);
0650         } else {
0651           ACTS_VERBOSE("Stop Kalman filtering with "
0652                        << result.collectedTracks.size() << " found tracks");
0653           result.finished = true;
0654         }
0655       }
0656     }
0657 
0658     /// @brief CombinatorialKalmanFilter actor operation: reset propagation
0659     ///
0660     /// @tparam propagator_state_t Type of Propagator state
0661     /// @tparam stepper_t Type of the stepper
0662     /// @tparam navigator_t Type of the navigator
0663     ///
0664     /// @param state is the mutable propagator state object
0665     /// @param stepper is the stepper in use
0666     /// @param navigator is the navigator in use
0667     /// @param result is the mutable result state object
0668     template <typename propagator_state_t, typename stepper_t,
0669               typename navigator_t>
0670     void reset(propagator_state_t& state, const stepper_t& stepper,
0671                const navigator_t& navigator, result_type& result) const {
0672       auto currentBranch = result.activeBranches.back();
0673       auto currentState = currentBranch.outermostTrackState();
0674 
0675       // Reset the stepping state
0676       stepper.resetState(state.stepping, currentState.filtered(),
0677                          currentState.filteredCovariance(),
0678                          currentState.referenceSurface(),
0679                          state.options.stepping.maxStepSize);
0680 
0681       // Reset the navigation state
0682       // Set targetSurface to nullptr for forward filtering
0683       auto navigationOptions = state.navigation.options;
0684       navigationOptions.startSurface = &currentState.referenceSurface();
0685       navigationOptions.targetSurface = nullptr;
0686       state.navigation = navigator.makeState(navigationOptions);
0687       navigator.initialize(state, stepper);
0688 
0689       // No Kalman filtering for the starting surface, but still need
0690       // to consider the material effects here
0691       materialInteractor(navigator.currentSurface(state.navigation), state,
0692                          stepper, navigator, MaterialUpdateStage::PostUpdate);
0693 
0694       detail::setupLoopProtection(state, stepper, result.pathLimitReached, true,
0695                                   logger());
0696     }
0697 
0698     /// @brief CombinatorialKalmanFilter actor operation:
0699     /// - filtering for all measurement(s) on surface
0700     /// - store selected track states in multiTrajectory
0701     /// - update propagator state to the (last) selected track state
0702     ///
0703     /// @tparam propagator_state_t Type of the Propagator state
0704     /// @tparam stepper_t Type of the stepper
0705     /// @tparam navigator_t Type of the navigator
0706     ///
0707     /// @param surface The surface where the update happens
0708     /// @param state The mutable propagator state object
0709     /// @param stepper The stepper in use
0710     /// @param navigator The navigator in use
0711     /// @param result The mutable result state object
0712     template <typename propagator_state_t, typename stepper_t,
0713               typename navigator_t>
0714     Result<void> filter(const Surface* surface, propagator_state_t& state,
0715                         const stepper_t& stepper, const navigator_t& navigator,
0716                         result_type& result) const {
0717       using PM = TrackStatePropMask;
0718 
0719       std::size_t nBranchesOnSurface = 0;
0720 
0721       if (auto [slBegin, slEnd] = m_sourcelinkAccessor(*surface);
0722           slBegin != slEnd) {
0723         // Screen output message
0724         ACTS_VERBOSE("Measurement surface " << surface->geometryId()
0725                                             << " detected.");
0726 
0727         // Transport the covariance to the surface
0728         stepper.transportCovarianceToBound(state.stepping, *surface);
0729 
0730         // Update state and stepper with pre material effects
0731         materialInteractor(surface, state, stepper, navigator,
0732                            MaterialUpdateStage::PreUpdate);
0733 
0734         // Bind the transported state to the current surface
0735         auto boundStateRes =
0736             stepper.boundState(state.stepping, *surface, false);
0737         if (!boundStateRes.ok()) {
0738           return boundStateRes.error();
0739         }
0740         auto& boundState = *boundStateRes;
0741         auto& [boundParams, jacobian, pathLength] = boundState;
0742         boundParams.covariance() = state.stepping.cov;
0743 
0744         auto currentBranch = result.activeBranches.back();
0745         TrackIndexType prevTip = currentBranch.tipIndex();
0746 
0747         // Create trackstates for all source links (will be filtered later)
0748         using TrackStatesResult =
0749             Acts::Result<CkfTypes::BranchVector<TrackIndexType>>;
0750         TrackStatesResult tsRes = trackStateCandidateCreator(
0751             state.geoContext, *calibrationContextPtr, *surface, boundState,
0752             slBegin, slEnd, prevTip, *result.trackStates,
0753             result.trackStateCandidates, *result.trackStates, logger());
0754         if (!tsRes.ok()) {
0755           ACTS_ERROR(
0756               "Processing of selected track states failed: " << tsRes.error());
0757           return tsRes.error();
0758         }
0759         const CkfTypes::BranchVector<TrackIndexType>& newTrackStateList =
0760             *tsRes;
0761 
0762         if (newTrackStateList.empty()) {
0763           ACTS_VERBOSE("Detected hole after measurement selection on surface "
0764                        << surface->geometryId());
0765 
0766           // Setting the number of branches on the surface to 1 as the hole
0767           // still counts as a branch
0768           nBranchesOnSurface = 1;
0769 
0770           auto stateMask = PM::Predicted | PM::Jacobian;
0771 
0772           // Add a hole track state to the multitrajectory
0773           TrackIndexType currentTip = addNonSourcelinkState(
0774               stateMask, boundState, result, true, prevTip);
0775           auto nonSourcelinkState =
0776               result.trackStates->getTrackState(currentTip);
0777           currentBranch.tipIndex() = currentTip;
0778           currentBranch.nHoles()++;
0779 
0780           BranchStopperResult branchStopperResult =
0781               m_extensions.branchStopper(currentBranch, nonSourcelinkState);
0782 
0783           // Check the branch
0784           if (branchStopperResult == BranchStopperResult::Continue) {
0785             // Remembered the active branch and its state
0786           } else {
0787             // No branch on this surface
0788             nBranchesOnSurface = 0;
0789             if (branchStopperResult == BranchStopperResult::StopAndKeep) {
0790               storeLastActiveBranch(result);
0791             }
0792             // Remove the branch from list
0793             result.activeBranches.pop_back();
0794           }
0795         } else {
0796           Result<unsigned int> procRes = processNewTrackStates(
0797               state.geoContext, newTrackStateList, result);
0798           if (!procRes.ok()) {
0799             ACTS_ERROR("Processing of selected track states failed: "
0800                        << procRes.error());
0801             return procRes.error();
0802           }
0803           nBranchesOnSurface = *procRes;
0804 
0805           if (nBranchesOnSurface == 0) {
0806             // All branches on the surface have been stopped. Reset happens at
0807             // the end of the function
0808             ACTS_VERBOSE("All branches on surface " << surface->geometryId()
0809                                                     << " have been stopped");
0810           } else {
0811             // `currentBranch` is invalidated after `processNewTrackStates`
0812             currentBranch = result.activeBranches.back();
0813             prevTip = currentBranch.tipIndex();
0814 
0815             auto currentState = currentBranch.outermostTrackState();
0816 
0817             if (currentState.typeFlags().test(TrackStateFlag::OutlierFlag)) {
0818               // We don't need to update the stepper given an outlier state
0819               ACTS_VERBOSE("Outlier state detected on surface "
0820                            << surface->geometryId());
0821             } else {
0822               // If there are measurement track states on this surface
0823               ACTS_VERBOSE("Filtering step successful with "
0824                            << nBranchesOnSurface << " branches");
0825               // Update stepping state using filtered parameters of last track
0826               // state on this surface
0827               stepper.update(state.stepping,
0828                              MultiTrajectoryHelpers::freeFiltered(
0829                                  state.options.geoContext, currentState),
0830                              currentState.filtered(),
0831                              currentState.filteredCovariance(), *surface);
0832               ACTS_VERBOSE(
0833                   "Stepping state is updated with filtered parameter:");
0834               ACTS_VERBOSE("-> " << currentState.filtered().transpose()
0835                                  << " of track state with tip = "
0836                                  << currentState.index());
0837             }
0838           }
0839         }
0840 
0841         // Update state and stepper with post material effects
0842         materialInteractor(surface, state, stepper, navigator,
0843                            MaterialUpdateStage::PostUpdate);
0844       } else if (surface->associatedDetectorElement() != nullptr ||
0845                  surface->surfaceMaterial() != nullptr) {
0846         // No splitting on the surface without source links. Set it to one
0847         // first, but could be changed later
0848         nBranchesOnSurface = 1;
0849 
0850         auto currentBranch = result.activeBranches.back();
0851         TrackIndexType prevTip = currentBranch.tipIndex();
0852 
0853         // The surface could be either sensitive or passive
0854         bool isSensitive = (surface->associatedDetectorElement() != nullptr);
0855         bool isMaterial = (surface->surfaceMaterial() != nullptr);
0856         ACTS_VERBOSE("Detected " << (isSensitive ? "sensitive" : "passive")
0857                                  << " surface: " << surface->geometryId());
0858         // Add state if there is already measurement detected on this branch
0859         if (currentBranch.nMeasurements() > 0 || isMaterial) {
0860           // No source links on surface, add either hole or passive material
0861           // TrackState. No storage allocation for uncalibrated/calibrated
0862           // measurement and filtered parameter
0863           auto stateMask = PM::Predicted | PM::Jacobian;
0864 
0865           // Transport the covariance to a curvilinear surface
0866           stepper.transportCovarianceToCurvilinear(state.stepping);
0867 
0868           // Update state and stepper with pre material effects
0869           materialInteractor(surface, state, stepper, navigator,
0870                              MaterialUpdateStage::PreUpdate);
0871 
0872           // Transport & bind the state to the current surface
0873           auto boundStateRes =
0874               stepper.boundState(state.stepping, *surface, false);
0875           if (!boundStateRes.ok()) {
0876             return boundStateRes.error();
0877           }
0878           auto& boundState = *boundStateRes;
0879           auto& [boundParams, jacobian, pathLength] = boundState;
0880           boundParams.covariance() = state.stepping.cov;
0881 
0882           // Add a hole or material track state to the multitrajectory
0883           TrackIndexType currentTip = addNonSourcelinkState(
0884               stateMask, boundState, result, isSensitive, prevTip);
0885           auto nonSourcelinkState =
0886               result.trackStates->getTrackState(currentTip);
0887           currentBranch.tipIndex() = currentTip;
0888           if (isSensitive) {
0889             currentBranch.nHoles()++;
0890           }
0891 
0892           BranchStopperResult branchStopperResult =
0893               m_extensions.branchStopper(currentBranch, nonSourcelinkState);
0894 
0895           // Check the branch
0896           if (branchStopperResult == BranchStopperResult::Continue) {
0897             // Remembered the active branch and its state
0898           } else {
0899             // No branch on this surface
0900             nBranchesOnSurface = 0;
0901             if (branchStopperResult == BranchStopperResult::StopAndKeep) {
0902               storeLastActiveBranch(result);
0903             }
0904             // Remove the branch from list
0905             result.activeBranches.pop_back();
0906           }
0907 
0908           // Update state and stepper with post material effects
0909           materialInteractor(surface, state, stepper, navigator,
0910                              MaterialUpdateStage::PostUpdate);
0911         }
0912       } else {
0913         // Neither measurement nor material on surface, this branch is still
0914         // valid. Count the branch on current surface
0915         nBranchesOnSurface = 1;
0916       }
0917 
0918       // Reset current branch if there is no branch on current surface
0919       if (nBranchesOnSurface == 0) {
0920         ACTS_DEBUG("Branch on surface " << surface->geometryId()
0921                                         << " is stopped");
0922         if (!result.activeBranches.empty()) {
0923           ACTS_VERBOSE("Propagation jumps to branch with tip = "
0924                        << result.activeBranches.back().tipIndex());
0925           reset(state, stepper, navigator, result);
0926         } else {
0927           ACTS_VERBOSE("Stop Kalman filtering with "
0928                        << result.collectedTracks.size() << " found tracks");
0929           result.finished = true;
0930         }
0931       }
0932 
0933       return Result<void>::success();
0934     }
0935 
0936     /// Process new, incompomplete track states and set the filtered state
0937     ///
0938     /// @note will process the given list of new states, run the updater
0939     ///     or share the predicted state for states flagged as outliers
0940     ///     and add them to the list of active branches
0941     ///
0942     /// @param gctx The geometry context for this track finding/fitting
0943     /// @param newTrackStateList index list of new track states
0944     /// @param result which contains among others the new states, and the list of active branches
0945     /// @return the number of newly added branches or an error
0946     Result<unsigned int> processNewTrackStates(
0947         const Acts::GeometryContext& gctx,
0948         const CkfTypes::BranchVector<TrackIndexType>& newTrackStateList,
0949         result_type& result) const {
0950       using PM = TrackStatePropMask;
0951 
0952       unsigned int nBranchesOnSurface = 0;
0953 
0954       auto rootBranch = result.activeBranches.back();
0955 
0956       // Build the new branches by forking the root branch. Reverse the order to
0957       // process the best candidate first
0958       CkfTypes::BranchVector<TrackProxy> newBranches;
0959       for (auto it = newTrackStateList.rbegin(); it != newTrackStateList.rend();
0960            ++it) {
0961         // Keep the root branch as the first branch, make a copy for the others
0962         auto newBranch = (it == newTrackStateList.rbegin())
0963                              ? rootBranch
0964                              : rootBranch.shallowCopy();
0965         newBranch.tipIndex() = *it;
0966         newBranches.push_back(newBranch);
0967       }
0968 
0969       // Remove the root branch
0970       result.activeBranches.pop_back();
0971 
0972       // Update and select from the new branches
0973       for (TrackProxy newBranch : newBranches) {
0974         auto trackState = newBranch.outermostTrackState();
0975         TrackStateType typeFlags = trackState.typeFlags();
0976 
0977         if (typeFlags.test(TrackStateFlag::OutlierFlag)) {
0978           // No Kalman update for outlier
0979           // Set the filtered parameter index to be the same with predicted
0980           // parameter
0981           trackState.shareFrom(PM::Predicted, PM::Filtered);
0982           // Increment number of outliers
0983           newBranch.nOutliers()++;
0984         } else if (typeFlags.test(TrackStateFlag::MeasurementFlag)) {
0985           // Kalman update
0986           auto updateRes =
0987               m_extensions.updater(gctx, trackState, *updaterLogger);
0988           if (!updateRes.ok()) {
0989             ACTS_ERROR("Update step failed: " << updateRes.error());
0990             return updateRes.error();
0991           }
0992           ACTS_VERBOSE("Appended measurement track state with tip = "
0993                        << newBranch.tipIndex());
0994           // Set the measurement flag
0995           typeFlags.set(TrackStateFlag::MeasurementFlag);
0996           // Increment number of measurements
0997           newBranch.nMeasurements()++;
0998           newBranch.nDoF() += trackState.calibratedSize();
0999           newBranch.chi2() += trackState.chi2();
1000         } else {
1001           ACTS_WARNING("Cannot handle this track state flags");
1002           continue;
1003         }
1004 
1005         result.activeBranches.push_back(newBranch);
1006 
1007         BranchStopperResult branchStopperResult =
1008             m_extensions.branchStopper(newBranch, trackState);
1009 
1010         // Check if need to stop this branch
1011         if (branchStopperResult == BranchStopperResult::Continue) {
1012           // Record the number of branches on surface
1013           nBranchesOnSurface++;
1014         } else {
1015           // Record the number of stopped branches
1016           if (branchStopperResult == BranchStopperResult::StopAndKeep) {
1017             storeLastActiveBranch(result);
1018           }
1019           // Remove the branch from list
1020           result.activeBranches.pop_back();
1021         }
1022       }
1023 
1024       return nBranchesOnSurface;
1025     }
1026 
1027     /// @brief CombinatorialKalmanFilter actor operation: add a hole or material track state
1028     ///
1029     /// @param stateMask The bitmask that instructs which components to allocate
1030     /// @param boundState The bound state on current surface
1031     /// @param result is the mutable result state object and which to leave invalid
1032     /// @param isSensitive The surface is sensitive or passive
1033     /// @param prevTip The index of the previous state
1034     ///
1035     /// @return The tip of added state
1036     TrackIndexType addNonSourcelinkState(TrackStatePropMask stateMask,
1037                                          const BoundState& boundState,
1038                                          result_type& result, bool isSensitive,
1039                                          TrackIndexType prevTip) const {
1040       using PM = TrackStatePropMask;
1041 
1042       // Add a track state
1043       auto trackStateProxy =
1044           result.trackStates->makeTrackState(stateMask, prevTip);
1045       ACTS_VERBOSE("Create " << (isSensitive ? "Hole" : "Material")
1046                              << " output track state #"
1047                              << trackStateProxy.index()
1048                              << " with mask: " << stateMask);
1049 
1050       const auto& [boundParams, jacobian, pathLength] = boundState;
1051       // Fill the track state
1052       trackStateProxy.predicted() = boundParams.parameters();
1053       trackStateProxy.predictedCovariance() = boundParams.covariance().value();
1054       trackStateProxy.jacobian() = jacobian;
1055       trackStateProxy.pathLength() = pathLength;
1056       // Set the surface
1057       trackStateProxy.setReferenceSurface(
1058           boundParams.referenceSurface().getSharedPtr());
1059 
1060       // Set the track state flags
1061       auto typeFlags = trackStateProxy.typeFlags();
1062       if (trackStateProxy.referenceSurface().surfaceMaterial() != nullptr) {
1063         typeFlags.set(TrackStateFlag::MaterialFlag);
1064       }
1065       typeFlags.set(TrackStateFlag::ParameterFlag);
1066       if (isSensitive) {
1067         typeFlags.set(TrackStateFlag::HoleFlag);
1068       }
1069 
1070       // Set the filtered parameter index to be the same with predicted
1071       // parameter
1072       trackStateProxy.shareFrom(PM::Predicted, PM::Filtered);
1073 
1074       return trackStateProxy.index();
1075     }
1076 
1077     /// @brief CombinatorialKalmanFilter actor operation: material interaction
1078     ///
1079     /// @tparam propagator_state_t is the type of Propagator state
1080     /// @tparam stepper_t Type of the stepper
1081     /// @tparam navigator_t Type of the navigator
1082     ///
1083     /// @param surface The surface where the material interaction happens
1084     /// @param state The mutable propagator state object
1085     /// @param stepper The stepper in use
1086     /// @param navigator The navigator in use
1087     /// @param updateStage The material update stage
1088     template <typename propagator_state_t, typename stepper_t,
1089               typename navigator_t>
1090     void materialInteractor(const Surface* surface, propagator_state_t& state,
1091                             const stepper_t& stepper,
1092                             const navigator_t& navigator,
1093                             const MaterialUpdateStage& updateStage) const {
1094       if (surface == nullptr) {
1095         return;
1096       }
1097 
1098       // Indicator if having material
1099       bool hasMaterial = false;
1100 
1101       if (surface->surfaceMaterial() != nullptr) {
1102         // Prepare relevant input particle properties
1103         detail::PointwiseMaterialInteraction interaction(surface, state,
1104                                                          stepper);
1105         // Evaluate the material properties
1106         if (interaction.evaluateMaterialSlab(state, navigator, updateStage)) {
1107           // Surface has material at this stage
1108           hasMaterial = true;
1109 
1110           // Evaluate the material effects
1111           interaction.evaluatePointwiseMaterialInteraction(multipleScattering,
1112                                                            energyLoss);
1113 
1114           // Screen out material effects info
1115           ACTS_VERBOSE("Material effects on surface: "
1116                        << surface->geometryId()
1117                        << " at update stage: " << updateStage << " are :");
1118           ACTS_VERBOSE("eLoss = "
1119                        << interaction.Eloss * interaction.navDir << ", "
1120                        << "variancePhi = " << interaction.variancePhi << ", "
1121                        << "varianceTheta = " << interaction.varianceTheta
1122                        << ", "
1123                        << "varianceQoverP = " << interaction.varianceQoverP);
1124 
1125           // Update the state and stepper with material effects
1126           interaction.updateState(state, stepper, addNoise);
1127         }
1128       }
1129 
1130       if (!hasMaterial) {
1131         // Screen out message
1132         ACTS_VERBOSE("No material effects on surface: " << surface->geometryId()
1133                                                         << " at update stage: "
1134                                                         << updateStage);
1135       }
1136     }
1137 
1138     void storeLastActiveBranch(result_type& result) const {
1139       auto currentBranch = result.activeBranches.back();
1140       TrackIndexType currentTip = currentBranch.tipIndex();
1141 
1142       ACTS_VERBOSE("Find track with entry index = "
1143                    << currentTip << " and there are nMeasurements = "
1144                    << currentBranch.nMeasurements()
1145                    << ", nOutliers = " << currentBranch.nOutliers()
1146                    << ", nHoles = " << currentBranch.nHoles() << " on track");
1147 
1148       std::optional<TrackStateProxy> lastMeasurement;
1149       for (const auto& trackState : currentBranch.trackStatesReversed()) {
1150         if (trackState.typeFlags().test(TrackStateFlag::MeasurementFlag)) {
1151           lastMeasurement = trackState;
1152           break;
1153         }
1154       }
1155 
1156       if (lastMeasurement.has_value()) {
1157         currentBranch.tipIndex() = lastMeasurement->index();
1158         result.collectedTracks.push_back(currentBranch);
1159         ACTS_VERBOSE("Last measurement found on track with entry index = "
1160                      << currentTip << " and measurement index = "
1161                      << lastMeasurement->index());
1162       } else {
1163         ACTS_VERBOSE(
1164             "No measurement found on track with entry index = " << currentTip);
1165       }
1166     }
1167 
1168     CombinatorialKalmanFilterExtensions<track_container_t> m_extensions;
1169 
1170     /// The source link accessor
1171     source_link_accessor_t m_sourcelinkAccessor;
1172 
1173     using source_link_iterator_t =
1174         decltype(std::declval<decltype(m_sourcelinkAccessor(
1175                      *static_cast<const Surface*>(nullptr)))>()
1176                      .first);
1177 
1178     using TrackStateCandidateCreator =
1179         typename CombinatorialKalmanFilterOptions<
1180             source_link_iterator_t,
1181             track_container_t>::TrackStateCandidateCreator;
1182 
1183     /// the stateCandidator to be used
1184     /// @note will be set to a default trackStateCandidateCreator or the one
1185     //        provided via the extension
1186     TrackStateCandidateCreator trackStateCandidateCreator;
1187 
1188     /// End of world aborter
1189     EndOfWorldReached endOfWorldReached;
1190 
1191     /// Actor logger instance
1192     const Logger* actorLogger{nullptr};
1193     /// Updater logger instance
1194     const Logger* updaterLogger{nullptr};
1195 
1196     const Logger& logger() const { return *actorLogger; }
1197   };
1198 
1199   template <typename source_link_accessor_t, typename parameters_t>
1200   class Aborter {
1201    public:
1202     /// Broadcast the action type
1203     using action_type = Actor<source_link_accessor_t, parameters_t>;
1204 
1205     template <typename propagator_state_t, typename stepper_t,
1206               typename navigator_t, typename result_t>
1207     bool operator()(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
1208                     const navigator_t& /*navigator*/, const result_t& result,
1209                     const Logger& /*logger*/) const {
1210       return !result.lastError.ok() || result.finished;
1211     }
1212   };
1213 
1214   /// Void path limit reached aborter to replace the default since the path
1215   /// limit is handled in the CKF actor internally.
1216   struct StubPathLimitReached {
1217     double internalLimit{};
1218 
1219     template <typename propagator_state_t, typename stepper_t,
1220               typename navigator_t>
1221     bool operator()(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
1222                     const navigator_t& /*navigator*/,
1223                     const Logger& /*logger*/) const {
1224       return false;
1225     }
1226   };
1227 
1228  public:
1229   /// Combinatorial Kalman Filter implementation, calls the Kalman filter
1230   ///
1231   /// @tparam source_link_iterator_t Type of the source link iterator
1232   /// @tparam start_parameters_t Type of the initial parameters
1233   /// @tparam parameters_t Type of parameters used for local parameters
1234   ///
1235   /// @param initialParameters The initial track parameters
1236   /// @param tfOptions CombinatorialKalmanFilterOptions steering the track
1237   ///                  finding
1238   /// @param trackContainer Input track container to use
1239   /// @note The input measurements are given in the form of @c SourceLinks.
1240   ///       It's @c calibrator_t's job to turn them into calibrated measurements
1241   ///       used in the track finding.
1242   ///
1243   /// @return a container of track finding result for all the initial track
1244   /// parameters
1245   template <typename source_link_iterator_t, typename start_parameters_t,
1246             typename parameters_t = BoundTrackParameters>
1247   auto findTracks(const start_parameters_t& initialParameters,
1248                   const CombinatorialKalmanFilterOptions<
1249                       source_link_iterator_t, track_container_t>& tfOptions,
1250                   track_container_t& trackContainer) const
1251       -> Result<std::vector<
1252           typename std::decay_t<decltype(trackContainer)>::TrackProxy>> {
1253     using SourceLinkAccessor =
1254         SourceLinkAccessorDelegate<source_link_iterator_t>;
1255 
1256     // Create the ActionList and AbortList
1257     using CombinatorialKalmanFilterAborter =
1258         Aborter<SourceLinkAccessor, parameters_t>;
1259     using CombinatorialKalmanFilterActor =
1260         Actor<SourceLinkAccessor, parameters_t>;
1261     using Actors = ActionList<CombinatorialKalmanFilterActor>;
1262     using Aborters = AbortList<CombinatorialKalmanFilterAborter>;
1263 
1264     // Create relevant options for the propagation options
1265     using PropagatorOptions =
1266         typename propagator_t::template Options<Actors, Aborters>;
1267     PropagatorOptions propOptions(tfOptions.geoContext,
1268                                   tfOptions.magFieldContext);
1269 
1270     // Set the trivial propagator options
1271     propOptions.setPlainOptions(tfOptions.propagatorPlainOptions);
1272 
1273     // Catch the actor
1274     auto& combKalmanActor =
1275         propOptions.actionList.template get<CombinatorialKalmanFilterActor>();
1276     combKalmanActor.targetReached.surface = tfOptions.targetSurface;
1277     combKalmanActor.multipleScattering = tfOptions.multipleScattering;
1278     combKalmanActor.energyLoss = tfOptions.energyLoss;
1279     combKalmanActor.skipPrePropagationUpdate =
1280         tfOptions.skipPrePropagationUpdate;
1281     combKalmanActor.actorLogger = m_actorLogger.get();
1282     combKalmanActor.updaterLogger = m_updaterLogger.get();
1283     combKalmanActor.calibrationContextPtr = &tfOptions.calibrationContext.get();
1284 
1285     // copy source link accessor, calibrator and measurement selector
1286     combKalmanActor.m_sourcelinkAccessor = tfOptions.sourcelinkAccessor;
1287     combKalmanActor.m_extensions = tfOptions.extensions;
1288     combKalmanActor.trackStateCandidateCreator =
1289         tfOptions.trackStateCandidateCreator;
1290     DefaultTrackStateCreator defaultTrackStateCreator;
1291     // connect a default state candidate creator if no state candidate creator
1292     // was provided via the extension
1293     if (!combKalmanActor.trackStateCandidateCreator.connected()) {
1294       defaultTrackStateCreator.calibrator = tfOptions.extensions.calibrator;
1295       defaultTrackStateCreator.measurementSelector =
1296           tfOptions.extensions.measurementSelector;
1297       combKalmanActor.trackStateCandidateCreator.template connect<
1298           &DefaultTrackStateCreator::template createSourceLinkTrackStates<
1299               source_link_iterator_t>>(&defaultTrackStateCreator);
1300     }
1301 
1302     auto propState =
1303         m_propagator.template makeState<start_parameters_t, PropagatorOptions,
1304                                         StubPathLimitReached>(initialParameters,
1305                                                               propOptions);
1306 
1307     auto& r =
1308         propState
1309             .template get<CombinatorialKalmanFilterResult<track_container_t>>();
1310     r.tracks = &trackContainer;
1311     r.trackStates = &trackContainer.trackStateContainer();
1312 
1313     auto rootBranch = trackContainer.makeTrack();
1314     r.activeBranches.push_back(rootBranch);
1315 
1316     auto propagationResult = m_propagator.propagate(propState);
1317 
1318     auto result = m_propagator.makeResult(
1319         std::move(propState), propagationResult, propOptions, false);
1320 
1321     if (!result.ok()) {
1322       ACTS_ERROR("Propagation failed: " << result.error() << " "
1323                                         << result.error().message()
1324                                         << " with the initial parameters: \n"
1325                                         << initialParameters.parameters());
1326       return result.error();
1327     }
1328 
1329     auto& propRes = *result;
1330 
1331     // Get the result of the CombinatorialKalmanFilter
1332     auto combKalmanResult =
1333         std::move(propRes.template get<
1334                   CombinatorialKalmanFilterResult<track_container_t>>());
1335 
1336     Result<void> error = combKalmanResult.lastError;
1337     if (error.ok() && !combKalmanResult.finished) {
1338       error = Result<void>(
1339           CombinatorialKalmanFilterError::PropagationReachesMaxSteps);
1340     }
1341     if (!error.ok()) {
1342       ACTS_ERROR("CombinatorialKalmanFilter failed: "
1343                  << combKalmanResult.lastError.error() << " "
1344                  << combKalmanResult.lastError.error().message()
1345                  << " with the initial parameters: "
1346                  << initialParameters.parameters().transpose());
1347       return error.error();
1348     }
1349 
1350     return std::move(combKalmanResult.collectedTracks);
1351   }
1352 };
1353 
1354 }  // namespace Acts