Allow KF components to return results

Also adds an aborter to the KF to be able to terminate fitting in case
an unrecoverable error occurs.

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(),

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();
   }
 

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);