diff --git a/Core/include/Acts/Fitter/GainMatrixSmoother.hpp b/Core/include/Acts/Fitter/GainMatrixSmoother.hpp index 31e4e1c3dcd301b4486dec6a4f5849e8965891bf..5c1f8d5c39d826ea905fb22e974c928e5eda8b6d 100644 --- a/Core/include/Acts/Fitter/GainMatrixSmoother.hpp +++ b/Core/include/Acts/Fitter/GainMatrixSmoother.hpp @@ -13,8 +13,8 @@ #include "Acts/EventData/MultiTrajectory.hpp" #include "Acts/EventData/TrackParameters.hpp" #include "Acts/Fitter/KalmanFitterError.hpp" -#include "Acts/Utilities/Result.hpp" #include "Acts/Utilities/Logger.hpp" +#include "Acts/Utilities/Result.hpp" namespace Acts { @@ -38,9 +38,9 @@ class GainMatrixSmoother { : m_logger(std::move(logger)) {} template <typename source_link_t> - parameters_t operator()(const GeometryContext& gctx, - MultiTrajectory<source_link_t>& trajectory, - size_t entryIndex) const { + Result<parameters_t> operator()(const GeometryContext& gctx, + MultiTrajectory<source_link_t>& trajectory, + size_t entryIndex) const { ACTS_VERBOSE("Invoked GainMatrixSmoother on entry index: " << entryIndex); using namespace boost::adaptors; @@ -59,13 +59,14 @@ class GainMatrixSmoother { gain_matrix_t G; // make sure there is more than one track state + std::optional<std::error_code> error{std::nullopt}; // assume ok if (prev_ts.previous() == Acts::detail_lt::IndexData::kInvalid) { ACTS_VERBOSE("Only one track state given, smoothing terminates early"); } else { ACTS_VERBOSE("Start smoothing from previous track state at index: " << prev_ts.previous()); - trajectory.applyBackwards(prev_ts.previous(), [&prev_ts, &G, + trajectory.applyBackwards(prev_ts.previous(), [&prev_ts, &G, &error, this](auto ts) { // should have filtered and predicted, this should also include the // covariances. @@ -88,6 +89,11 @@ class GainMatrixSmoother { G = ts.filteredCovariance() * ts.jacobian().transpose() * prev_ts.predictedCovariance().inverse(); + if (G.hasNaN()) { + error = KalmanFitterError::SmoothFailed; // set to error + return false; // abort execution + } + ACTS_VERBOSE("Gain smoothing matrix G:\n" << G); ACTS_VERBOSE("Calculate smoothed parameters:"); @@ -116,8 +122,13 @@ class GainMatrixSmoother { ACTS_VERBOSE("Smoothed covariance is: \n" << ts.smoothedCovariance()); prev_ts = ts; + return true; // continue execution }); } + if (error) { + // error is set, return result + return *error; + } // construct parameters from last track state parameters_t lastSmoothed(gctx, prev_ts.smoothedCovariance(), diff --git a/Core/include/Acts/Fitter/GainMatrixUpdater.hpp b/Core/include/Acts/Fitter/GainMatrixUpdater.hpp index 3841bcd0677c5a5248f72cc7184ff016877a4435..b6cdd4f500956896ff4a71732f6be45326aa56ab 100644 --- a/Core/include/Acts/Fitter/GainMatrixUpdater.hpp +++ b/Core/include/Acts/Fitter/GainMatrixUpdater.hpp @@ -134,6 +134,7 @@ class GainMatrixUpdater { ACTS_VERBOSE("Chi2: " << trackState.chi2()); }); + // always succeed, no outlier logic yet return Result<void>::success(); } diff --git a/Core/include/Acts/Fitter/KalmanFitter.hpp b/Core/include/Acts/Fitter/KalmanFitter.hpp index e55c4a94492ff97c63661f53f806ac0af29e4c91..4582be4b4cb04c98809e6aade7fb327605491062 100644 --- a/Core/include/Acts/Fitter/KalmanFitter.hpp +++ b/Core/include/Acts/Fitter/KalmanFitter.hpp @@ -174,6 +174,29 @@ class KalmanFitter { /// Owned logging instance std::shared_ptr<const Logger> m_logger; + template <typename source_link_t> + class Aborter { + public: + /// Broadcast the result_type + using result_type = KalmanFitterResult<source_link_t>; + + template <typename propagator_state_t, typename stepper_t> + bool operator()(propagator_state_t& /*state*/, + const stepper_t& /*unused*/) const { + return false; + } + + template <typename propagator_state_t, typename stepper_t, + typename result_t> + bool operator()(const result_t& result, propagator_state_t& /*state*/, + const stepper_t& /*stepper*/) const { + if (!result.result.ok()) { + return true; + } + return false; + } + }; + /// @brief Propagator Actor plugin for the KalmanFilter /// /// @tparam source_link_t is an type fulfilling the @c SourceLinkConcept @@ -383,7 +406,7 @@ class KalmanFitter { << " filtered track states."); } // Smooth the track states and obtain the last smoothed track parameters - auto smoothRes = + auto smoothRes = m_smoother(state.geoContext, result.fittedStates, result.trackTip); if (!smoothRes.ok()) { ACTS_ERROR("Smoothing step failed: " << smoothRes.error()); @@ -458,7 +481,7 @@ class KalmanFitter { /// @return the output as an output track template <typename source_link_t, typename start_parameters_t, typename parameters_t = BoundParameters> - Result<KalmanFitterResult<source_link_t, parameters_t>> fit( + Result<KalmanFitterResult<source_link_t>> fit( const std::vector<source_link_t>& sourcelinks, const start_parameters_t& sParameters, const KalmanFitterOptions& kfOptions) const { @@ -475,7 +498,7 @@ class KalmanFitter { } // Create the ActionList and AbortList - using KalmanAborter = Aborter<source_link_t, parameters_t>; + using KalmanAborter = Aborter<source_link_t>; using KalmanActor = Actor<source_link_t, parameters_t>; using KalmanResult = typename KalmanActor::result_type; using Actors = ActionList<KalmanActor>; @@ -491,6 +514,10 @@ class KalmanFitter { kalmanActor.inputMeasurements = std::move(inputMeasurements); kalmanActor.targetSurface = kfOptions.referenceSurface; + // also set logger on updater and smoother + kalmanActor.m_updater.m_logger = kalmanActor.m_logger; + kalmanActor.m_smoother.m_logger = kalmanActor.m_logger; + // Run the fitter auto result = m_propagator.template propagate(sParameters, kalmanOptions);