Add logging to GainMatrix*, reuse logger from KF

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/Logger.hpp"
 #include "Acts/Utilities/Result.hpp"
+#include "Acts/Utilities/Logger.hpp"
 
 namespace Acts {
 
@@ -41,7 +41,7 @@ class GainMatrixSmoother {
   parameters_t operator()(const GeometryContext& gctx,
                           MultiTrajectory<source_link_t>& trajectory,
                           size_t entryIndex) const {
-    ACTS_VERBOSE("Invoked GainMatrixSmoother");
+    ACTS_VERBOSE("Invoked GainMatrixSmoother on entry index: " << entryIndex);
     using namespace boost::adaptors;
 
     // using ParVector_t = typename parameters_t::ParVector_t;
@@ -58,33 +58,48 @@ class GainMatrixSmoother {
     // Smoothing gain matrix
     gain_matrix_t G;
 
-    trajectory.applyBackwards(prev_ts.previous(), [&prev_ts, &G](auto ts) {
-      // should have filtered and predicted, this should also include the
-      // covariances.
-      assert(ts.hasFiltered());
-      assert(ts.hasPredicted());
-      assert(ts.hasJacobian());
-
-      // previous trackstate should have smoothed and predicted
-      assert(prev_ts.hasSmoothed());
-      assert(prev_ts.hasPredicted());
-
-      // Gain smoothing matrix
-      G = ts.filteredCovariance() * ts.jacobian().transpose() *
-          prev_ts.predictedCovariance().inverse();
-
-      // Calculate the smoothed parameters
-      ts.smoothed() =
-          ts.filtered() + G * (prev_ts.smoothed() - prev_ts.predicted());
-
-      // And the smoothed covariance
-      ts.smoothedCovariance() =
-          ts.filteredCovariance() -
-          G * (prev_ts.predictedCovariance() - prev_ts.smoothedCovariance()) *
-              G.transpose();
-
-      prev_ts = ts;
-    });
+    // make sure there is more than one track state
+    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,
+                                                     this](auto ts) {
+        // should have filtered and predicted, this should also include the
+        // covariances.
+        assert(ts.hasFiltered());
+        assert(ts.hasPredicted());
+        assert(ts.hasJacobian());
+
+        // previous trackstate should have smoothed and predicted
+        assert(prev_ts.hasSmoothed());
+        assert(prev_ts.hasPredicted());
+
+        // Gain smoothing matrix
+        G = ts.filteredCovariance() * ts.jacobian().transpose() *
+            prev_ts.predictedCovariance().inverse();
+
+        ACTS_VERBOSE("Gain smoothing matrix is:\n" << G);
+
+        // Calculate the smoothed parameters
+        ts.smoothed() =
+            ts.filtered() + G * (prev_ts.smoothed() - prev_ts.predicted());
+
+        ACTS_VERBOSE("Smoothed parameters are: " << ts.smoothed().transpose());
+
+        // And the smoothed covariance
+        ts.smoothedCovariance() =
+            ts.filteredCovariance() -
+            G * (prev_ts.predictedCovariance() - prev_ts.smoothedCovariance()) *
+                G.transpose();
+
+        ACTS_VERBOSE("Smoothed covariance is: \n" << ts.smoothedCovariance());
+
+        prev_ts = ts;
+      });
+    }
 
     // construct parameters from last track state
     parameters_t lastSmoothed(gctx, prev_ts.smoothedCovariance(),

Core/include/Acts/Fitter/GainMatrixUpdater.hpp

@@ -77,8 +77,9 @@ class GainMatrixUpdater {
     const auto predicted = trackState.predicted();
     const auto predicted_covariance = trackState.predictedCovariance();
 
-    // ParVector_t filtered_parameters;
-    // CovMatrix_t filtered_covariance;
+    ACTS_VERBOSE("Predicted parameters: " << predicted.transpose());
+    ACTS_VERBOSE("Predicted covariance:\n" << predicted_covariance);
+
     // read-write handles. Types are eigen maps into backing storage.
     // This writes directly into the trajectory storage
     auto filtered = trackState.filtered();
@@ -92,31 +93,45 @@ class GainMatrixUpdater {
           using cov_t = ActsSymMatrixD<measdim>;
           using par_t = ActsVectorD<measdim>;
 
+          ACTS_VERBOSE("Measurement dimension: " << measdim);
+          ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose());
+          ACTS_VERBOSE("Calibrated measurement covariance:\n"
+                       << calibrated_covariance);
+
           const ActsMatrixD<measdim, BoundParsDim> H =
               trackState.projector()
                   .template topLeftCorner<measdim, BoundParsDim>();
 
+          ACTS_VERBOSE("Measurement projector H:\n" << H);
+
           const ActsMatrixD<BoundParsDim, measdim> K =
               predicted_covariance * H.transpose() *
               (H * predicted_covariance * H.transpose() + calibrated_covariance)
                   .inverse();
 
+          ACTS_VERBOSE("Gain Matrix K:\n" << K);
+
           filtered = predicted + K * (calibrated - H * predicted);
           filtered_covariance =
               (ActsSymMatrixD<
                    MultiTrajectory<SourceLink>::ParametersSize>::Identity() -
                K * H) *
               predicted_covariance;
+          ACTS_VERBOSE("Filtered parameters: " << filtered.transpose());
+          ACTS_VERBOSE("Filtered covariance:\n" << filtered_covariance);
 
           // calculate filtered residual
           par_t residual(trackState.calibratedSize());
           residual = (calibrated - H * filtered);
+          ACTS_VERBOSE("Residual: " << residual.transpose());
 
           trackState.chi2() =
               (residual.transpose() *
                ((cov_t::Identity() - H * K) * calibrated_covariance).inverse() *
                residual)
                   .value();
+
+          ACTS_VERBOSE("Chi2: " << trackState.chi2());
         });
 
     return Result<void>::success();
@@ -130,6 +145,12 @@ class GainMatrixUpdater {
     assert(m_logger);
     return *m_logger;
   }
+
+  /// Pointer to a logger that is owned by the parent, KalmanFilter
+  const Logger* m_logger{nullptr};
+
+  /// Getter for the logger, to support logging macros
+  const Logger& logger() const { return *m_logger; }
 };
 
 }  // namespace Acts