Chi2Fitter.h

Go to the documentation of this file.

/*
  Copyright (C) 2002-2026 CERN for the benefit of the ATLAS collaboration
*/
#ifndef MUONTRACKFINDINGTOOLS_FITTER_H
#define MUONTRACKFINDINGTOOLS_FITTER_H
 
#include "Acts/Definitions/Algebra.hpp"
#include "Acts/Definitions/Common.hpp"
#include "Acts/EventData/MeasurementHelpers.hpp"
#include "Acts/EventData/MultiTrajectory.hpp"
#include "Acts/EventData/SourceLink.hpp"
#include "Acts/EventData/TrackProxyConcept.hpp"
#include "Acts/EventData/Types.hpp"
#include "Acts/EventData/VectorMultiTrajectory.hpp"
#include "Acts/EventData/VectorTrackContainer.hpp"
#include "Acts/EventData/detail/CorrectedTransformationFreeToBound.hpp"
#include "Acts/Geometry/GeometryContext.hpp"
#include "Acts/Geometry/TrackingVolume.hpp"
#include "Acts/MagneticField/MagneticFieldContext.hpp"
#include "Acts/Material/Interactions.hpp"
#include "Acts/Propagator/DirectNavigator.hpp"
#include "Acts/Propagator/PropagatorOptions.hpp"
#include "Acts/Propagator/StandardAborters.hpp"
#include "Acts/Propagator/detail/PointwiseMaterialInteraction.hpp"
#include "Acts/TrackFitting/GlobalChiSquareFitterError.hpp"
#include "Acts/TrackFitting/detail/VoidFitterComponents.hpp"
#include "Acts/Utilities/CalibrationContext.hpp"
#include "Acts/Utilities/Delegate.hpp"
#include "Acts/Utilities/Logger.hpp"
#include "Acts/Utilities/Result.hpp"
#include "Acts/Utilities/TrackHelpers.hpp"
 
#include <functional>
#include <limits>
#include <memory>
#include <type_traits>
#include <unordered_map>
 
using namespace Acts;
namespace MuonExperimental {

struct Gx2fSystem;
 
namespace Gx2fConstants {
constexpr std::string_view gx2fnUpdateColumn = "Gx2fnUpdateColumn";
 
// Mask for the track states. We don't need Predicted and Filtered
constexpr TrackStatePropMask trackStateMask = TrackStatePropMask::Smoothed |
                                              TrackStatePropMask::Jacobian |
                                              TrackStatePropMask::Calibrated;
 
}  // namespace Gx2fConstants

struct Gx2fMaterialProperties {
  struct ELossAtSurface {
    constexpr bool isValid() const { return m_isValid; }

    constexpr double lostEnergy() const { return m_lostEnergy; }
    constexpr double lostSigma() const { return m_lossSigma; }

    ELossAtSurface() = default;

    explicit ELossAtSurface(const double energy, const double sigma) noexcept
        : m_isValid{std::min(energy, sigma) > Acts::s_epsilon},
          m_lostEnergy{energy},
          m_lossSigma{sigma} {}

    ELossAtSurface& operator+=(const ELossAtSurface& other) noexcept {
      if (&other != this && other.isValid()) {
        m_lostEnergy += other.m_lostEnergy;
        m_lossSigma = Acts::fastHypot(m_lossSigma, other.lostSigma());
        m_isValid = true;
      }
      return *this;
    }

    static ELossAtSurface invalidELoss(const GeometryContext& geoContext,
                                       const CalibrationContext& calibContext,
                                       const Vector3& pos, const Vector3& dir,
                                       const Surface& matSurface);
 
   private:
    bool m_isValid{false};
    double m_lostEnergy{0.};
    double m_lossSigma{0.};
  };

  struct ScatteringAtSurface {
    constexpr bool isValid() const { return m_isValid; }
    constexpr double sigma() const { return m_sigma; }
    ScatteringAtSurface() = default;
    explicit ScatteringAtSurface(const double sigma)
        : m_isValid{sigma > Acts::s_epsilon}, m_sigma{sigma} {}
 
    ScatteringAtSurface& operator+=(const ScatteringAtSurface& other) {
      if (&other != this && other.isValid()) {
        m_sigma += other.sigma();
        m_isValid = true;
      }
 
      return *this;
    }

    bool m_isValid{false};
    double m_sigma{0.};
  };

  explicit Gx2fMaterialProperties(ScatteringAtSurface&& scatterer,
                                  ELossAtSurface&& eLoss)
      : m_scatterer{std::move(scatterer)},
        m_eloss{std::move(eLoss)},
        m_lostEnergy{m_eloss.isValid() ? m_eloss.lostEnergy() : 0.} {}

  constexpr bool materialIsValid() const {
    return m_scatterer.isValid() || m_eloss.isValid();
  }

  constexpr std::size_t nDim() const {
    return (m_scatterer.isValid() ? 2ul : 0ul) +
           (m_eloss.isValid() ? 1ul : 0ul);
  }
 
  const ScatteringAtSurface& scatterer() const { return m_scatterer; }
 
  const ELossAtSurface& energyLoss() const { return m_eloss; }
 
  constexpr double deltaTheta() const { return m_scatTheta; }
 
  constexpr double deltaPhi() const { return m_scatPhi; }
 
  constexpr double lostEnergy() const { return m_lostEnergy; }
 
  constexpr double qOverPderiv() const { return m_qOverPderiv; }
 
  constexpr double qOverPratio() const { return m_qOverPratio; }

  //  subtract the energy loss from the q/p parameters
  void updateTrackParameters(BoundTrackParameters& trackPars);
 
  void contributionToGx2fSums(Gx2fSystem& extendedSystem, const double theta,
                              const std::size_t firstParamIdx,
                              const Logger& logger) const;
 
  void updateParameters(const Eigen::VectorXd& deltaParamsExtended,
                        const std::size_t firstParamIdx);
 
 private:
  ScatteringAtSurface m_scatterer{};
  ELossAtSurface m_eloss{};


  double m_lostEnergy{0.};
  double m_scatTheta{0.};
  double m_scatPhi{0.};
  double m_qOverPderiv{0.};
  // qoverP ratio after and before the energy loss
  double m_qOverPratio{1.};
};

template <typename traj_t>
struct Gx2FitterExtensions {
  using TrackStateProxy = typename MultiTrajectory<traj_t>::TrackStateProxy;
  using ConstTrackStateProxy =
      typename MultiTrajectory<traj_t>::ConstTrackStateProxy;
  using Parameters = typename TrackStateProxy::Parameters;

  using Calibrator =
      Delegate<void(const GeometryContext&, const CalibrationContext&,
                    const SourceLink&, TrackStateProxy)>;

  using OutlierFinder = Delegate<bool(ConstTrackStateProxy)>;

  using ELossAccumulator = Delegate<Gx2fMaterialProperties::ELossAtSurface(
      const GeometryContext& geoContext, const CalibrationContext& calibContext,
      const Vector3& pos, const Vector3& dir, const Surface& matSurf)>;

  ELossAccumulator elossAccumulator;
  Calibrator calibrator;

  OutlierFinder outlierFinder;

  SourceLinkSurfaceAccessor surfaceAccessor;

  Gx2FitterExtensions() {
    calibrator.template connect<&Acts::detail::voidFitterCalibrator<traj_t>>();
    outlierFinder.template connect<&Acts::detail::voidOutlierFinder<traj_t>>();
    surfaceAccessor.connect<&Acts::detail::voidSurfaceAccessor>();
    elossAccumulator
        .connect<&Gx2fMaterialProperties::ELossAtSurface::invalidELoss>();
  }
};

template <typename traj_t>
struct Gx2FitterOptions {
  Gx2FitterOptions(const GeometryContext& gctx,
                   const MagneticFieldContext& mctx,
                   std::reference_wrapper<const CalibrationContext> cctx,
                   Gx2FitterExtensions<traj_t> extensions_,
                   const PropagatorPlainOptions& pOptions)
      : geoContext(gctx),
        magFieldContext(mctx),
        calibrationContext(cctx),
        extensions(std::move(extensions_)),
        propagatorPlainOptions(pOptions){}
 

  std::reference_wrapper<const GeometryContext> geoContext;
  std::reference_wrapper<const MagneticFieldContext> magFieldContext;
  std::reference_wrapper<const CalibrationContext> calibrationContext;

  Gx2FitterExtensions<traj_t> extensions;

  PropagatorPlainOptions propagatorPlainOptions;

  bool doMultipleScattering = false;

  bool doEnergyLoss = false;

  bool includeMaterialInIter = true;

  double scatteringCutOff = 0.;
  double elossCutOff = 0.;

  FreeToBoundCorrection freeToBoundCorrection{false};

  std::size_t nUpdateMax = 5;

  double relChi2changeCutOff = 1e-5;
};

template <typename traj_t>
struct Gx2FitterResult {
  traj_t* fittedStates{nullptr};

  std::unordered_map<GeometryIdentifier, Gx2fMaterialProperties> materialMap{};

  Gx2fMaterialProperties::ELossAtSurface accEloss{};
  Gx2fMaterialProperties::ScatteringAtSurface accScatterer{};

  std::size_t lastMeasurementIndex = Acts::kTrackIndexInvalid;

  std::size_t lastTrackIndex = Acts::kTrackIndexInvalid;

  std::size_t measurementStates{0ul};

  std::size_t measurementHoles{0ul};

  std::size_t processedStates{0ul};

  std::size_t processedMeasurements{0ul};

  std::vector<std::size_t> materialParamIndices{eBoundSize};

  bool finished = false;

  std::vector<const Surface*> missedActiveSurfaces;

  std::size_t surfaceCount = 0;
};

struct Gx2fSystem {
  explicit Gx2fSystem(std::size_t nDims) : m_nDims{nDims} {}

  std::size_t nDims() const { return m_nDims; }

  double chi2() const { return m_chi2; }

  double& chi2() { return m_chi2; }

  const Eigen::MatrixXd& aMatrix() const { return m_aMatrix; }

  Eigen::MatrixXd& aMatrix() { return m_aMatrix; }

  const Eigen::VectorXd& bVector() const { return m_bVector; }

  Eigen::VectorXd& bVector() { return m_bVector; }

  std::size_t ndf() const { return m_ndf; }

  std::size_t& ndf() { return m_ndf; }

  std::size_t findRequiredNdf() {
    std::size_t ndfSystem = 0;
    if (m_aMatrix(4, 4) == 0) {
      ndfSystem = 4;
    } else if (m_aMatrix(5, 5) == 0) {
      ndfSystem = 5;
    } else {
      ndfSystem = 6;
    }
 
    return ndfSystem;
  }

  bool isWellDefined() { return m_ndf > findRequiredNdf(); }
 
 private:
  std::size_t m_nDims;

  double m_chi2 = 0.;

  Eigen::MatrixXd m_aMatrix{Eigen::MatrixXd::Zero(m_nDims, m_nDims)};

  Eigen::VectorXd m_bVector{Eigen::VectorXd::Zero(m_nDims)};

  std::size_t m_ndf = 0u;
};

void addMeasurementToGx2fSumsBackend(
    Gx2fSystem& extendedSystem,
    const std::vector<BoundMatrix>& jacobianFromStart,
    const std::vector<std::size_t>& materialIndices,
    const std::vector<double>& qOverPderiv,
    const Eigen::MatrixXd& covarianceMeasurement, const BoundVector& predicted,
    const Eigen::VectorXd& measurement, const Eigen::MatrixXd& projector,
    const Logger& logger);

template <std::size_t kMeasDim, typename track_state_t>
void addMeasurementToGx2fSums(Gx2fSystem& extendedSystem,
                              const std::vector<BoundMatrix>& jacobianFromStart,
                              const std::vector<std::size_t>& materialIndices,
                              const std::vector<double>& qOverPderiv,
                              const track_state_t& trackState,
                              const Logger& logger) {
  const SquareMatrix<kMeasDim> covarianceMeasurement =
      trackState.template calibratedCovariance<kMeasDim>();
 
  const BoundVector predicted = trackState.smoothed();
 
  const Vector<kMeasDim> measurement =
      trackState.template calibrated<kMeasDim>();
 
  const Matrix<kMeasDim, eBoundSize> projector =
      trackState.template projectorSubspaceHelper<kMeasDim>().projector();
 
  addMeasurementToGx2fSumsBackend(
      extendedSystem, jacobianFromStart, materialIndices, qOverPderiv,
      covarianceMeasurement, predicted, measurement, projector, logger);
}

template <TrackProxyConcept track_proxy_t>
void fillGx2fSystem(
    const track_proxy_t track, Gx2fSystem& extendedSystem,
    const std::unordered_map<GeometryIdentifier, Gx2fMaterialProperties>&
        materialMap,
    const std::vector<std::size_t>& materialParamIndices,
    std::vector<GeometryIdentifier>& geoIdVector, const Logger& logger) {
  std::vector<BoundMatrix> jacobianFromStart;
  jacobianFromStart.emplace_back(BoundMatrix::Identity());
  geoIdVector.reserve(track.nTrackStates());
  std::vector<double> qOverPderiv;
 
  bool foundELoss{false};
  for (const auto& trackState : track.trackStates()) {
    // Get and store geoId for the current surface
    const GeometryIdentifier geoId = trackState.referenceSurface().geometryId();
    ACTS_DEBUG("Start to investigate trackState on surface " << geoId);
    const auto typeFlags = trackState.typeFlags();
 
    // update all Jacobians from start for every track state
    for (auto& jac : jacobianFromStart) {
      jac = trackState.jacobian() * jac;
    }
 
    // Skip the hole states
    if (!typeFlags.hasMeasurement() && !typeFlags.hasMaterial()) {
      ACTS_DEBUG("    Skip state.");
      continue;
    }
 
    // Handle measurement
    if (typeFlags.hasMeasurement()) {
      ACTS_DEBUG("    Handle measurement. ");
 
      const auto measDim = trackState.calibratedSize();
 
      if (measDim < 1 || 6 < measDim) {
        ACTS_ERROR("Can not process state with measurement with "
                   << measDim << " dimensions.");
        throw std::domain_error(
            "Found measurement with less than 1 or more than 6 dimension(s).");
      }
 
      extendedSystem.ndf() += measDim;
 
      visit_measurement(measDim, [&](auto N) {
        addMeasurementToGx2fSums<N>(extendedSystem, jacobianFromStart,
                                    materialParamIndices, qOverPderiv,
                                    trackState, logger);
      });
    }
 
    // Handle material
    if (typeFlags.hasMaterial()) {
      ACTS_DEBUG("    Handle material");
      const auto materialMapId = materialMap.find(geoId);
      if (materialMapId == materialMap.end()) {
        ACTS_ERROR("No material properties found for surface " << geoId);
        throw std::runtime_error("No material properties found for surface.");
      }
      if (materialMapId->second.nDim() == 0ul) {
        continue;
      }
      if (materialMapId->second.energyLoss().isValid()) {
        ACTS_VERBOSE("The material surface carries energy loss");
        foundELoss = true;
      }
      const std::size_t deltaPosition =
          materialParamIndices.at(geoIdVector.size());
      geoIdVector.push_back(geoId);
 
      materialMapId->second.contributionToGx2fSums(
          extendedSystem, trackState.smoothed()[eBoundTheta], deltaPosition,
          logger);
 
      // after material surfaces, the measurements depend on the updated qoverp
      // we need to scale the jacobian term for the qoverp row
      if (foundELoss) {
        ACTS_DEBUG("    Update Jacobian for q/p after material surface "
                   << geoId << " with ratio " << materialMapId->second.qOverPratio());
        jacobianFromStart.front().row(eBoundQOverP) *=
            materialMapId->second.qOverPratio();
      }
      // Add for this material a new Jacobian, starting from this surface.
      jacobianFromStart.emplace_back(BoundMatrix::Identity());
      qOverPderiv.push_back(materialMapId->second.qOverPderiv());
    }
  }
}

Eigen::VectorXd computeGx2fDeltaParams(const Gx2fSystem& extendedSystem);

void updateGx2fParams(
    BoundTrackParameters& params, const Eigen::VectorXd& deltaParamsExtended,
    std::unordered_map<GeometryIdentifier, Gx2fMaterialProperties>& materialMap,
    const std::vector<GeometryIdentifier>& geoIdVector);

void updateGx2fCovarianceParams(BoundMatrix& fullCovariancePredicted,
                                Gx2fSystem& extendedSystem);

template <typename propagator_t, typename traj_t>
class Gx2Fitter {
  using Gx2fNavigator = typename propagator_t::Navigator;
 
  static constexpr auto kInvalid = kTrackIndexInvalid;
 
 public:
  explicit Gx2Fitter(const propagator_t& pPropagator,
                     std::unique_ptr<const Logger> log =
                         getDefaultLogger("Gx2Fitter", Logging::INFO))
      : m_propagator(pPropagator), m_logger{std::move(log)} {}
 
 private:
  propagator_t m_propagator;

  std::unique_ptr<const Logger> m_logger;
  std::unique_ptr<const Logger> m_actorLogger{
      m_logger->cloneWithSuffix("Actor")};
  std::unique_ptr<const Logger> m_addToSumLogger{
      m_logger->cloneWithSuffix("AddToSum")};
 
  const Logger& logger() const { return *m_logger; }

  class GX2FActor {
   public:
    using result_type = Gx2FitterResult<traj_t>;
 
    using TrackStateProxy_t = typename traj_t::TrackStateProxy;

    const std::unordered_map<const Surface*, SourceLink>* inputMeasurements{};

    bool doMultipleScattering = false;

    bool doEnergyLoss = false;
 
    double scatteringCutOff = 0.;
    double elossCutOff = 0.;

    FreeToBoundCorrection freeToBoundCorrection;
    const Logger* actorLogger{nullptr};

    const Logger& logger() const { return *actorLogger; }
 
    Gx2FitterExtensions<traj_t> extensions;

    const CalibrationContext* calibrationContext{nullptr};

    const BoundTrackParameters* startParameters = nullptr;

    template <typename propagator_state_t, typename stepper_t,
              typename navigator_t>
    Result<void> act(propagator_state_t& state, const stepper_t& stepper,
                     const navigator_t& navigator, result_type& result,
                     const Logger& /*logger*/) const {
      assert(result.fittedStates && "No MultiTrajectory set");
 
      // Check if we can stop to propagate
      if (result.measurementStates == inputMeasurements->size()) {
        ACTS_DEBUG("Actor: finish: All measurements have been found.");
        result.finished = true;
      } else if (state.navigation.navigationBreak) {
        ACTS_DEBUG("Actor: finish: navigationBreak.");
        result.finished = true;
      }
 
      // End the propagation and return to the fitter
      if (result.finished) {
        // Remove the missing surfaces that occur after the last measurement
        if (result.measurementStates > 0) {
          result.missedActiveSurfaces.resize(result.measurementHoles);
        }
 
        return Result<void>::success();
      }
 
      // We are only interested in surfaces. If we are not on a surface, we
      // continue the navigation
      const Surface* surface = navigator.currentSurface(state.navigation);
      if (surface == nullptr) {
        return Result<void>::success();
      }
 
      ++result.surfaceCount;
      const GeometryIdentifier geoId = surface->geometryId();
      ACTS_DEBUG("Surface "
                 << geoId << ", " << surface->bounds() << " detected. "
                 << "Sensitive: " << (surface->isSensitive() ? "yes" : "no")
                 << ", has material: "
                 << (surface->hasMaterial() ? "yes" : "no"));
 
      auto matFillResult = fillMaterialMap(state, stepper, navigator, result);
      if (!matFillResult.ok()) {
        return matFillResult.error();
      }
 
      auto trackStateResult = makeTrackState(state, stepper, navigator, result);
      if (!trackStateResult.ok()) {
        return trackStateResult.error();
      }
      // Here we handle all measurements
      if (auto sourceLinkIt = inputMeasurements->find(surface);
          sourceLinkIt != inputMeasurements->end()) {
        // We count the processed measurement
        ++result.processedMeasurements;
 
        result.lastMeasurementIndex = result.lastTrackIndex;
 
        // TODO check for outlier first
        // We count the state with measurement
        ++result.measurementStates;
 
        // We count the processed state
        ++result.processedStates;
 
        // Update the number of holes count only when encountering a
        // measurement
        result.measurementHoles = result.missedActiveSurfaces.size();
 
        return Result<void>::success();
      }
 
      ACTS_DEBUG("    The surface contains no measurement/material/hole.");
      return Result<void>::success();
    }
 
    template <typename propagator_state_t, typename stepper_t,
              typename navigator_t>
    Result<void> makeTrackState(propagator_state_t& state,
                                const stepper_t& stepper,
                                const navigator_t& navigator,
                                result_type& result) const {
      const Surface* surface = navigator.currentSurface(state.navigation);
 
      const GeometryIdentifier geoId = surface->geometryId();
 
      auto sourceLinkIt = inputMeasurements->find(surface);
      auto materialItr = result.materialMap.find(geoId);
      const bool isSensitive =
          surface->isSensitive() || sourceLinkIt != inputMeasurements->end();
      const bool hasMaterial = surface->hasMaterial() &&
                               materialItr != result.materialMap.end() &&
                               materialItr->second.materialIsValid();
      if (!isSensitive && !hasMaterial) {
        return Result<void>::success();
      }
 
      auto& fittedStates = *result.fittedStates;
 
      // Add a <trackStateMask> TrackState entry multi trajectory. This
      // allocates storage for all components, which we will set later.
      TrackStateProxy_t trackStateProxy = fittedStates.makeTrackState(
          Gx2fConstants::trackStateMask, result.lastTrackIndex);
 
      result.lastTrackIndex = trackStateProxy.index();
 
      auto typeFlags = trackStateProxy.typeFlags();
      typeFlags.setHasParameters();
 
      // Transport the covariance to the surface
      auto transportRes = stepper.transportToBound(
          state.stepping, *surface, freeToBoundCorrection);
      if (!transportRes.ok()) {
        return transportRes.error();
      }
 
      // Set the trackStateProxy components with the state from the ongoing
      // propagation
      trackStateProxy.setReferenceSurface(surface->getSharedPtr());
 
      // Not const since, we might need to update with scattering angles
      auto res = stepper.boundParameters(state.stepping, *surface);
      if (!res.ok()) {
        return res.error();
      }
      // Not const since, we might need to update with scattering angles
      auto& boundParams = *res;
 
      trackStateProxy.smoothedCovariance() = stepper.covariance(state.stepping).value();
      trackStateProxy.jacobian() = *transportRes;
      trackStateProxy.pathLength() = stepper.pathLength(state.stepping);

      if (hasMaterial) {
        typeFlags.setHasMaterial();
 
        // track
        ACTS_DEBUG("    Update parameters with material effects.");
        ACTS_VERBOSE("        boundParams before the update: "
                     << boundParams.parameters().transpose());
 
        materialItr->second.updateTrackParameters(boundParams);
        ACTS_VERBOSE("        boundParams after the update: "
                     << boundParams.parameters().transpose());
 
        stepper.update(state.stepping,
                       transformBoundToFreeParameters(
                           trackStateProxy.referenceSurface(), state.geoContext,
                           boundParams.parameters()),
                       boundParams.parameters(),
                       trackStateProxy.smoothedCovariance(), *surface);
      }
 
      // Fill the track state
      trackStateProxy.smoothed() = boundParams.parameters();
      if (!isSensitive) {
        return Result<void>::success();
      }
 
      if (sourceLinkIt == inputMeasurements->end()) {
        typeFlags.setIsHole();
        return Result<void>::success();
      }
      typeFlags.setIsMeasurement();
 
      // We have smoothed parameters, so calibrate the uncalibrated input
      // measurement
      extensions.calibrator(state.geoContext, *calibrationContext,
                            sourceLinkIt->second, trackStateProxy);
 
      return Result<void>::success();
    }


 
    template <typename propagator_state_t, typename stepper_t,
              typename navigator_t>
    Result<void> fillMaterialMap(propagator_state_t& state,
                                 const stepper_t& stepper,
                                 const navigator_t& navigator,
                                 result_type& result) const {
      if (!doMultipleScattering && !doEnergyLoss) {
        ACTS_DEBUG(
            __func__
            << " | "
            << "Multiple scattering and energy loss are disabled. Returning");
        return Result<void>::success();
      }
      const Surface* surface = navigator.currentSurface(state.navigation);
      if (!surface->hasMaterial()) {
        return Result<void>::success();
      }
 
      // Found material - add a scatteringAngles entry if not done yet and
      // energy loss if enabled. Handling will happen later
      ACTS_DEBUG("    The surface contains material ...");
 
      const GeometryIdentifier geoId = surface->geometryId();
      auto materialMapId = result.materialMap.find(geoId);
 
      if (materialMapId != result.materialMap.end()) {
        ACTS_VERBOSE("   ....  entry is already in the material map.");
        if (materialMapId->second.nDim() == 0ul) {
          ACTS_VERBOSE(" Material surface "
                       << geoId
                       << " has zero dimension - do not keep the index.");
          return Result<void>::success();
        }
        result.materialParamIndices.push_back(
            result.materialParamIndices.back() + materialMapId->second.nDim());
        return Result<void>::success();
      }
 
      ACTS_DEBUG("    ... create entry in material map.");
 
      const Result<MaterialSlab> slabResult =
          Acts::detail::evaluateMaterialSlab(
              state, stepper, *surface,
              Acts::detail::determineMaterialUpdateMode(
                  state, navigator, MaterialUpdateMode::FullUpdate));
      if (!slabResult.ok()) {
        ACTS_DEBUG(__func__ << " | "
                            << "Failed to evaluate material slab: "
                            << slabResult.error());
        return Result<void>::failure(slabResult.error());
      }
      const MaterialSlab& slab = *slabResult;
      const bool goodSlab = !slab.isVacuum();
 
      using ScatteringAtSurface = Gx2fMaterialProperties::ScatteringAtSurface;
      using ELossAtSurface = Gx2fMaterialProperties::ELossAtSurface;
 
      ELossAtSurface eLoss{};
      ScatteringAtSurface scatterer{};
 
      if (goodSlab) {
        const auto& particle = startParameters->particleHypothesis();
 
        const double sigma =
            static_cast<double>(Acts::computeMultipleScatteringTheta0(
                slab, particle.absolutePdg(), particle.mass(),
                static_cast<float>(startParameters->parameters()[eBoundQOverP]),
                particle.absoluteCharge()));
        ACTS_VERBOSE("        The Highland formula gives sigma = " << sigma);
 
        scatterer = ScatteringAtSurface{sigma};
 
      } else {
        ACTS_VERBOSE("        Material slab is not valid.");
      }
 
      if (doEnergyLoss) {
        eLoss = extensions.elossAccumulator(
            state.geoContext, *calibrationContext,
            stepper.position(state.stepping), stepper.direction(state.stepping),
            *surface);
        if (!eLoss.isValid() && goodSlab) {
          const auto& particle = startParameters->particleHypothesis();
 
          const double qOverP = stepper.qOverP(state.stepping);
          ACTS_VERBOSE("The particle has momentum "
                       << (1. / qOverP) * particle.absoluteCharge()
                       << " and charge " << particle.absoluteCharge());
 
          eLoss = ELossAtSurface{
              computeEnergyLossMean(slab, particle.absolutePdg(),
                                    particle.mass(), qOverP,
                                    particle.absoluteCharge()),
              static_cast<double>(Acts::computeEnergyLossLandauSigma(
                  slab, particle.mass(), static_cast<float>(qOverP),
                  particle.absoluteCharge()))};
 
          ACTS_VERBOSE("        The Bethe-Bloch formula gives energy loss "
                       << eLoss.lostEnergy() << " with uncertainty "
                       << eLoss.lostSigma());
        }
      }

      scatterer += result.accScatterer;
      eLoss += result.accEloss;
      if (scatterer.sigma() < scatteringCutOff) {
        result.accScatterer = std::move(scatterer);
        scatterer = ScatteringAtSurface{};
      } else {
        result.accScatterer = ScatteringAtSurface{};
      }
 
      if (eLoss.lostEnergy() < elossCutOff) {
        result.accEloss = std::move(eLoss);
        eLoss = ELossAtSurface{};
      } else {
        result.accEloss = ELossAtSurface{};
      }
      ACTS_DEBUG("    ... add entry in material map with scattering "
                 << scatterer.sigma() << " and energy loss "
                 << eLoss.lostEnergy());
      materialMapId =
          result.materialMap
              .emplace(geoId, Gx2fMaterialProperties{std::move(scatterer),
                                                     std::move(eLoss)})
              .first;
      if (materialMapId->second.nDim() == 0ul) {
        ACTS_VERBOSE(" Material surface "
                     << geoId
                     << " has zero dimension - do not keep the index.");
        return Result<void>::success();
      }
      result.materialParamIndices.push_back(result.materialParamIndices.back() +
                                            materialMapId->second.nDim());
 
      return Result<void>::success();
    }
 
    template <typename propagator_state_t, typename stepper_t,
              typename navigator_t, typename result_t>
    bool checkAbort(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
                    const navigator_t& /*navigator*/, const result_t& result,
                    const Logger& /*logger*/) const {
      return result.finished;
    }
  };
 
 public:
  template <typename source_link_iterator_t,
            TrackContainerFrontend track_container_t>
  Result<typename track_container_t::TrackProxy> fit(
      source_link_iterator_t it, source_link_iterator_t end,
      const BoundTrackParameters& sParameters,
      const Gx2FitterOptions<traj_t>& gx2fOptions,
      track_container_t& trackContainer) const
    requires(!std::is_same_v<Gx2fNavigator, DirectNavigator>)
  {
    // Preprocess Measurements (SourceLinks -> map)
    // To be able to find measurements later, we put them into a map.
    // We need to copy input SourceLinks anyway, so the map can own them.
    ACTS_VERBOSE("Preparing " << std::distance(it, end)
                              << " input measurements");
    std::unordered_map<const Surface*, SourceLink> inputMeasurements{};
 
    for (; it != end; ++it) {
      inputMeasurements.try_emplace(gx2fOptions.extensions.surfaceAccessor(*it),
                                    *it);
    }
 
    using GX2FResult = typename GX2FActor::result_type;
    using Actors = Acts::ActorList<GX2FActor>;
 
    using PropagatorOptions = typename propagator_t::template Options<Actors>;
 
    BoundTrackParameters params = sParameters;
    params.covariance() = BoundMatrix::Identity();
    // params.
    double chi2sum = 0;
    double oldChi2sum = std::numeric_limits<double>::max();
 
    // We need to create a temporary track container. We create several times a
    // new track and delete it after updating the parameters. However, if we
    // would work on the externally provided track container, it would be
    // difficult to remove the correct track, if it contains more than one.
    typename track_container_t::TrackContainerBackend trackContainerTempBackend;
    traj_t trajectoryTempBackend;
    TrackContainer trackContainerTemp{trackContainerTempBackend,
                                      trajectoryTempBackend};
 
    // Create an index of the 'tip' of the track stored in multitrajectory. It
    // is needed outside the update loop. It will be updated with each iteration
    // and used for the final track
    std::size_t tipIndex = kInvalid;
 
    // The materialMap stores for each visited surface their scattering
    // properties
    std::unordered_map<GeometryIdentifier, Gx2fMaterialProperties> materialMap;
 
    // This will be filled during the updates with the final covariance of the
    // track parameters.
    BoundMatrix fullCovariancePredicted = BoundMatrix::Identity();
 
    ACTS_VERBOSE("Initial parameters: " << params.parameters().transpose());

    ACTS_DEBUG("Start to iterate");
 
    // Iterate the fit and improve result. Abort after n steps or after
    // convergence.
    // nUpdate is initialized outside to save its state for the track.
    std::size_t nUpdate = 0;
    for (nUpdate = 0; nUpdate < gx2fOptions.nUpdateMax; ++nUpdate) {
      ACTS_DEBUG("nUpdate = " << nUpdate + 1 << "/" << gx2fOptions.nUpdateMax);
 
      // set up propagator and co
      PropagatorOptions propagatorOptions{gx2fOptions.propagatorPlainOptions};
 
      // Add the measurement surface as external surface to the navigator.
      // We will try to hit those surface by ignoring boundary checks.
      for (const auto& [surface, _] : inputMeasurements) {
        propagatorOptions.navigation.registerMeasurementSurface(*surface);
      }
 
      auto& gx2fActor = propagatorOptions.actorList.template get<GX2FActor>();
      gx2fActor.inputMeasurements = &inputMeasurements;
      if (gx2fOptions.includeMaterialInIter) {
        gx2fActor.doMultipleScattering = gx2fOptions.doMultipleScattering;
        gx2fActor.doEnergyLoss = gx2fOptions.doEnergyLoss;
        gx2fActor.scatteringCutOff = gx2fOptions.scatteringCutOff;
        gx2fActor.elossCutOff = gx2fOptions.elossCutOff;
      }
      gx2fActor.extensions = gx2fOptions.extensions;
      gx2fActor.calibrationContext = &gx2fOptions.calibrationContext.get();
      gx2fActor.actorLogger = m_actorLogger.get();
      gx2fActor.startParameters = &params;
 
      auto propagatorState = m_propagator.makeState(propagatorOptions);
 
      auto propagatorInitResult =
          m_propagator.initialize(propagatorState, params, nullptr);
      if (!propagatorInitResult.ok()) {
        ACTS_DEBUG("Propagation initialization failed: "
                   << propagatorInitResult.error());
        return propagatorInitResult.error();
      }
 
      auto& r = propagatorState.template get<Gx2FitterResult<traj_t>>();
      r.fittedStates = &trajectoryTempBackend;
      r.materialMap = std::move(materialMap);
 
      // Clear the track container. It could be more performant to update the
      // existing states, but this needs some more thinking.
      trackContainerTemp.clear();
 
      // Run the fitter
      auto propagationResult = m_propagator.propagate(propagatorState);
 
      auto result =
          m_propagator.makeResult(std::move(propagatorState), propagationResult,
                                  propagatorOptions, false);
 
      if (!result.ok()) {
        ACTS_DEBUG("Propagation failed: " << result.error());
        return result.error();
      }
 
      // TODO Improve Propagator + Actor [allocate before loop], rewrite
      // makeMeasurements
      auto& propRes = *result;
      GX2FResult gx2fResult = std::move(propRes.template get<GX2FResult>());
      materialMap = std::move(gx2fResult.materialMap);
 
      auto track = trackContainerTemp.makeTrack();
      tipIndex = gx2fResult.lastMeasurementIndex;
 
      // It could happen, that no measurements were found. Then the track would
      // be empty and the following operations would be invalid. Usually, this
      // only happens during the first iteration, due to bad initial parameters.
      if (tipIndex == kInvalid) {
        ACTS_INFO("Did not find any measurements in nUpdate "
                  << nUpdate + 1 << "/" << gx2fOptions.nUpdateMax);
        return Experimental::GlobalChiSquareFitterError::NotEnoughMeasurements;
      }
 
      track.tipIndex() = tipIndex;
      track.linkForward();
 
      // System that we fill with the information gathered by the actor and
      // evaluate later
      Gx2fSystem extendedSystem{gx2fResult.materialParamIndices.back()};
 
      // This vector stores the IDs for each visited material in order. We use
      // it later for updating the scattering angles. We cannot use
      // materialMap directly, since we cannot guarantee, that we will visit
      // all stored material in each propagation.
      std::vector<GeometryIdentifier> geoIdVector;
 
      fillGx2fSystem(track, extendedSystem, materialMap,
                     gx2fResult.materialParamIndices, geoIdVector,
                     *m_addToSumLogger);
 
      chi2sum = extendedSystem.chi2();
 
      // This check takes into account the evaluated dimensions of the
      // measurements. To fit, we need at least NDF+1 measurements. However, we
      // count n-dimensional measurements for n measurements, reducing the
      // effective number of needed measurements. We might encounter the case,
      // where we cannot use some (parts of a) measurements, maybe if we do not
      // support that kind of measurement. This is also taken into account here.
      // We skip the check during the first iteration, since we cannot guarantee
      // to hit all/enough measurement surfaces with the initial parameter
      // guess.
      // We skip the check during the first iteration, since we cannot guarantee
      // to hit all/enough measurement surfaces with the initial parameter
      // guess.
      if ((nUpdate > 0) && !extendedSystem.isWellDefined()) {
        ACTS_INFO("Not enough measurements. Require "
                  << extendedSystem.findRequiredNdf() + 1 << ", but only "
                  << extendedSystem.ndf() << " could be used.");
        return Experimental::GlobalChiSquareFitterError::NotEnoughMeasurements;
      }
 
      Eigen::VectorXd deltaParamsExtended =
          computeGx2fDeltaParams(extendedSystem);
 
      ACTS_VERBOSE("aMatrix:\n"
                   << extendedSystem.aMatrix() << "\n"
                   << "bVector:\n"
                   << extendedSystem.bVector() << "\n"
                   << "deltaParamsExtended:\n"
                   << deltaParamsExtended << "\n"
                   << "oldChi2sum = " << oldChi2sum << "\n"
                   << "chi2sum = " << extendedSystem.chi2());
 
      if ((gx2fOptions.relChi2changeCutOff != 0) && (nUpdate > 0) &&
          (std::abs(extendedSystem.chi2() / oldChi2sum - 1) <
           gx2fOptions.relChi2changeCutOff)) {
        ACTS_DEBUG("Abort with relChi2changeCutOff after "
                   << nUpdate + 1 << "/" << gx2fOptions.nUpdateMax
                   << " iterations.");
        updateGx2fCovarianceParams(fullCovariancePredicted, extendedSystem);
        break;
      }
 
      if (extendedSystem.chi2() > oldChi2sum + 1e-5) {
        ACTS_DEBUG("chi2 not converging monotonically in update " << nUpdate);
      }
 
      // If this is the final iteration, update the covariance and break.
      // Otherwise, we would update the scattering angles too much.
      if (nUpdate == gx2fOptions.nUpdateMax - 1) {
        // Since currently most of our tracks converge in 4-5 updates, we want
        // to set nUpdateMax higher than that to guarantee convergence for most
        // tracks. In cases, where we set a smaller nUpdateMax, it's because we
        // want to investigate the behaviour of the fitter before it converges,
        // like in some unit-tests.
        if (gx2fOptions.nUpdateMax > 5) {
          ACTS_INFO("Did not converge in " << gx2fOptions.nUpdateMax
                                           << " updates.");
          return Experimental::GlobalChiSquareFitterError::DidNotConverge;
        }
 
        updateGx2fCovarianceParams(fullCovariancePredicted, extendedSystem);
        break;
      }
 
      updateGx2fParams(params, deltaParamsExtended, materialMap, geoIdVector);
      ACTS_VERBOSE("Updated parameters: " << params.parameters().transpose());
 
      oldChi2sum = extendedSystem.chi2();
    }
    ACTS_DEBUG("Iterations finished");
    ACTS_VERBOSE("Final parameters: " << params.parameters().transpose());
    ACTS_VERBOSE("Final scattering angles and energy loss:");
    for (const auto& [key, value] : materialMap) {
      if (!value.materialIsValid()) {
        continue;
      }
      ACTS_VERBOSE("    ( " << value.deltaTheta() << " | " << value.deltaPhi()
                            << "|" << value.lostEnergy() << " )");
    }

    if (!gx2fOptions.includeMaterialInIter &&
        (gx2fOptions.doEnergyLoss || gx2fOptions.doMultipleScattering)) {
      ACTS_DEBUG("Start to evaluate material");
 
      // Set up the propagator
      PropagatorOptions propagatorOptions{gx2fOptions.propagatorPlainOptions};
 
      // Add the measurement surface as external surface to the navigator.
      // We will try to hit those surface by ignoring boundary checks.
      for (const auto& [surface, _] : inputMeasurements) {
        propagatorOptions.navigation.registerMeasurementSurface(*surface);
      }
 
      auto& gx2fActor = propagatorOptions.actorList.template get<GX2FActor>();
      gx2fActor.inputMeasurements = &inputMeasurements;
      gx2fActor.doMultipleScattering = gx2fOptions.doMultipleScattering;
      gx2fActor.doEnergyLoss = gx2fOptions.doEnergyLoss;
      gx2fActor.extensions = gx2fOptions.extensions;
      gx2fActor.calibrationContext = &gx2fOptions.calibrationContext.get();
      gx2fActor.actorLogger = m_actorLogger.get();
      gx2fActor.startParameters = &params;
      gx2fActor.scatteringCutOff = gx2fOptions.scatteringCutOff;
      gx2fActor.elossCutOff = gx2fOptions.elossCutOff;
 
      auto propagatorState = m_propagator.makeState(propagatorOptions);
 
      auto propagatorInitResult =
          m_propagator.initialize(propagatorState, params, nullptr);
      if (!propagatorInitResult.ok()) {
        ACTS_DEBUG("Propagation initialization failed: "
                   << propagatorInitResult.error());
        return propagatorInitResult.error();
      }
 
      auto& r = propagatorState.template get<Gx2FitterResult<traj_t>>();
      r.fittedStates = &trajectoryTempBackend;
      r.materialMap = std::move(materialMap);
 
      // Clear the track container. It could be more performant to update the
      // existing states, but this needs some more thinking.
      trackContainerTemp.clear();
 
      // Run the fitter
      auto propagationResult = m_propagator.propagate(propagatorState);
 
      auto result =
          m_propagator.makeResult(std::move(propagatorState), propagationResult,
                                  propagatorOptions, false);
 
      if (!result.ok()) {
        ACTS_DEBUG("Propagation failed: " << result.error());
        return result.error();
      }
 
      // TODO Improve Propagator + Actor [allocate before loop], rewrite
      // makeMeasurements
      auto& propRes = *result;
      GX2FResult gx2fResult = std::move(propRes.template get<GX2FResult>());
      materialMap = std::move(gx2fResult.materialMap);
      auto track = trackContainerTemp.makeTrack();
      tipIndex = gx2fResult.lastMeasurementIndex;
 
      // It could happen, that no measurements were found. Then the track would
      // be empty and the following operations would be invalid. Usually, this
      // only happens during the first iteration, due to bad initial parameters.
      if (tipIndex == kInvalid) {
        ACTS_INFO("Did not find any measurements in material fit.");
        return Experimental::GlobalChiSquareFitterError::NotEnoughMeasurements;
      }
 
      track.tipIndex() = tipIndex;
      track.linkForward();
 
      // System that we fill with the information gathered by the actor and
      // evaluate later
      Gx2fSystem extendedSystem{gx2fResult.materialParamIndices.back()};
 
      // This vector stores the IDs for each visited material in order. We use
      // it later for updating the scattering angles. We cannot use
      // materialMap directly, since we cannot guarantee, that we will visit
      // all stored material in each propagation.
      std::vector<GeometryIdentifier> geoIdVector;
 
      fillGx2fSystem(track, extendedSystem, materialMap,
                     gx2fResult.materialParamIndices, geoIdVector,
                     *m_addToSumLogger);
 
      chi2sum = extendedSystem.chi2();
 
      // This check takes into account the evaluated dimensions of the
      // measurements. To fit, we need at least NDF+1 measurements. However, we
      // count n-dimensional measurements for n measurements, reducing the
      // effective number of needed measurements. We might encounter the case,
      // where we cannot use some (parts of a) measurements, maybe if we do not
      // support that kind of measurement. This is also taken into account here.
      // We skip the check during the first iteration, since we cannot guarantee
      // to hit all/enough measurement surfaces with the initial parameter
      // guess.
      if ((nUpdate > 0) && !extendedSystem.isWellDefined()) {
        ACTS_INFO("Not enough measurements. Require "
                  << extendedSystem.findRequiredNdf() + 1 << ", but only "
                  << extendedSystem.ndf() << " could be used.");
        return Experimental::GlobalChiSquareFitterError::NotEnoughMeasurements;
      }
 
      Eigen::VectorXd deltaParamsExtended =
          computeGx2fDeltaParams(extendedSystem);
 
      ACTS_VERBOSE("aMatrix:\n"
                   << extendedSystem.aMatrix() << "\n"
                   << "bVector:\n"
                   << extendedSystem.bVector() << "\n"
                   << "deltaParamsExtended:\n"
                   << deltaParamsExtended << "\n"
                   << "oldChi2sum = " << oldChi2sum << "\n"
                   << "chi2sum = " << extendedSystem.chi2());
 
      chi2sum = extendedSystem.chi2();
 
      updateGx2fParams(params, deltaParamsExtended, materialMap, geoIdVector);
      ACTS_VERBOSE("Updated parameters: " << params.parameters().transpose());
 
      updateGx2fCovarianceParams(fullCovariancePredicted, extendedSystem);
 
      ACTS_DEBUG("Finished to evaluate material");
      ACTS_VERBOSE("Final parameters after material: "
                   << params.parameters().transpose());
 
      ACTS_VERBOSE("Final scattering angles and energy loss:");
      for (const auto& [key, value] : materialMap) {
        if (!value.materialIsValid()) {
          continue;
        }
        ACTS_VERBOSE("    ( " << value.deltaTheta() << " | " << value.deltaPhi()
                              << "|" << value.lostEnergy() << " )");
      }
    }
    ACTS_VERBOSE("Final covariance:\n" << fullCovariancePredicted);
 
    // Propagate again with the final covariance matrix. This is necessary to
    // obtain the propagated covariance for each state.
    // We also need to recheck the result and find the tipIndex, because at this
    // step, we will not ignore the boundary checks for measurement surfaces. We
    // want to create trackstates only on surfaces, that we actually hit.
    if (false && gx2fOptions.nUpdateMax > 0) {
      ACTS_VERBOSE("Propagate with the final covariance.");
      // update covariance
      params.covariance() = fullCovariancePredicted;
 
      // set up the propagator
      PropagatorOptions propagatorOptions{gx2fOptions.propagatorPlainOptions};
      auto& gx2fActor = propagatorOptions.actorList.template get<GX2FActor>();
      gx2fActor.inputMeasurements = &inputMeasurements;
      gx2fActor.doMultipleScattering = gx2fOptions.doMultipleScattering;
      gx2fActor.doEnergyLoss = gx2fOptions.doEnergyLoss;
      gx2fActor.extensions = gx2fOptions.extensions;
      gx2fActor.calibrationContext = &gx2fOptions.calibrationContext.get();
      gx2fActor.actorLogger = m_actorLogger.get();
      gx2fActor.startParameters = &params;
 
      auto propagatorState = m_propagator.makeState(propagatorOptions);
 
      auto propagatorInitResult =
          m_propagator.initialize(propagatorState, params, nullptr);
      if (!propagatorInitResult.ok()) {
        ACTS_DEBUG("Propagation initialization failed: "
                   << propagatorInitResult.error());
        return propagatorInitResult.error();
      }
 
      auto& r = propagatorState.template get<Gx2FitterResult<traj_t>>();
      r.fittedStates = &trackContainer.trackStateContainer();
      r.materialMap = std::move(materialMap);
 
      // Run the fitter
      auto propagationResult = m_propagator.propagate(propagatorState);
 
      auto result =
          m_propagator.makeResult(std::move(propagatorState), propagationResult,
                                  propagatorOptions, false);
 
      if (!result.ok()) {
        ACTS_DEBUG("Propagation failed: " << result.error());
        return result.error();
      }
 
      auto& propRes = *result;
      GX2FResult gx2fResult = std::move(propRes.template get<GX2FResult>());
      materialMap = std::move(gx2fResult.materialMap);
 
      if (tipIndex != gx2fResult.lastMeasurementIndex) {
        ACTS_INFO("Final fit used unreachable measurements.");
        tipIndex = gx2fResult.lastMeasurementIndex;
 
        // It could happen, that no measurements were found. Then the track
        // would be empty and the following operations would be invalid.
        if (tipIndex == kInvalid) {
          ACTS_INFO("Did not find any measurements in final propagation.");
          return Experimental::GlobalChiSquareFitterError::
              NotEnoughMeasurements;
        }
      }
    }
 
    if (!trackContainer.hasColumn(
            Acts::hashString(Gx2fConstants::gx2fnUpdateColumn))) {
      trackContainer.template addColumn<std::uint32_t>("Gx2fnUpdateColumn");
    }
 
    // Prepare track for return
    auto track = trackContainer.makeTrack();
    track.tipIndex() = tipIndex;
    track.parameters() = params.parameters();
    track.covariance() = fullCovariancePredicted;
    track.setReferenceSurface(params.referenceSurface().getSharedPtr());
 
    if (trackContainer.hasColumn(
            Acts::hashString(Gx2fConstants::gx2fnUpdateColumn))) {
      ACTS_DEBUG("Add nUpdate to track");
      track.template component<std::uint32_t>("Gx2fnUpdateColumn") =
          static_cast<std::uint32_t>(nUpdate);
    }
 
    // TODO write test for calculateTrackQuantities
    calculateTrackQuantities(track);
 
    // Set the chi2sum for the track summary manually, since we don't calculate
    // it for each state
    track.chi2() = chi2sum;
 
    track.linkForward();
 
    // Return the converted Track
    return track;
  }
};

 
}
#endif