Add derivative of track parameters to alignment parameters (#125)

* Add bone structure for KF-based alignment implementation
* Add calculation of global covariance matrix in KF
* Add option to calcuate global track params covariance
* Add enum for alignment parameters
* Add alignment derivatives engine
* Add derivative of bound parameters w.r.t. reference frame rotation
* Add enum for cartesian coordinate indices
* Move rotation derivatives into separate function
* Add layer and volume alignment to bound parameters derivative
* Change alignment frame to local frame
* Break alignment to bound derivative calculation into separate functions
* Fix the alignmentToPathDerivative return
* Move alignment derivative calculations to surface method
* Add static check of alignment parameters defintion
* Implement local3D to bound 2D derivative with separte method
* Re-implement calculation of alignment to path derivative for LineSurface
* Re-implement the local3D to bound local derivative for DiscSurface
* Re-implement the local3D to bound local derivative for LineSurface
* Re-implement the local3D to bound local derivative for CylinderSurface
* Re-implement the local3D to bound local derivative for ConeSurface
* Make the local3D to bound local derivative method pure virtual
* Add unit test for alignment helper
* Pass pointer for global covariance matrix to smoother
* Not use inline for alignment-related derivatives
* Move function definitions in AlignmentHelper.hpp to cpp
* Add derivative test for plane surface
* Add test for line surface derivative calculation
* Add derivative test for DiscSurface
* Add derivative test for CylinderSurface
* Add derivative test for ConeSurface
* Add more Surface tests
* Implement function to calculate global track parameters covariance
* Remove the option to consider only measurement states

Core/include/Acts/Fitter/GainMatrixSmoother.hpp

@@ -36,6 +36,17 @@ class GainMatrixSmoother {
           getDefaultLogger("GainMatrixSmoother", Logging::INFO).release()))
       : m_logger(std::move(logger)) {}
 
+  /// Operater for Kalman smoothing
+  ///
+  /// @tparam source_link_t The type of source link
+  ///
+  /// @param gctx The geometry context for the smoothing
+  /// @param trajectory The trajectory to be smoothed
+  /// @param entryIndex The index of state to start the smoothing
+  /// @param globalTrackParamsCovPtr The pointer to global track parameters
+  /// covariance matrix
+  ///
+  /// @return The smoothed track parameters at the first measurement state
   template <typename source_link_t>
   Result<parameters_t> operator()(const GeometryContext& gctx,
                                   MultiTrajectory<source_link_t>& trajectory,
@@ -44,8 +55,8 @@ class GainMatrixSmoother {
     using namespace boost::adaptors;
 
     // using ParVector_t = typename parameters_t::ParVector_t;
-    using CovMatrix_t = typename parameters_t::CovMatrix_t;
-    using gain_matrix_t = CovMatrix_t;
+    using CovMatrix = typename parameters_t::CovMatrix_t;
+    using GainMatrix = CovMatrix;
 
     // For the last state: smoothed is filtered - also: switch to next
     ACTS_VERBOSE("Getting previous track state");
@@ -55,7 +66,7 @@ class GainMatrixSmoother {
     prev_ts.smoothedCovariance() = prev_ts.filteredCovariance();
 
     // Smoothing gain matrix
-    gain_matrix_t G;
+    GainMatrix G;
 
     // make sure there is more than one track state
     std::optional<std::error_code> error{std::nullopt};  // assume ok
@@ -124,8 +135,8 @@ class GainMatrixSmoother {
         // If not, make one (could do more) attempt to replace it with the
         // nearest semi-positive def matrix,
         // but it could still be non semi-positive
-        CovMatrix_t smoothedCov = ts.smoothedCovariance();
-        if (not detail::covariance_helper<CovMatrix_t>::validate(smoothedCov)) {
+        CovMatrix smoothedCov = ts.smoothedCovariance();
+        if (not detail::covariance_helper<CovMatrix>::validate(smoothedCov)) {
           ACTS_DEBUG(
               "Smoothed covariance is not positive definite. Could result in "
               "negative covariance!");

Core/include/Acts/Fitter/KalmanFitter.hpp

@@ -99,7 +99,7 @@ struct KalmanFitterOptions {
   /// Whether to consider energy loss
   bool energyLoss = true;
 
-  /// Whether to run backward filtering.
+  /// Whether to run backward filtering
   bool backwardFiltering = false;
 };
 
@@ -870,10 +870,10 @@ class KalmanFitter {
         ACTS_VERBOSE("Apply smoothing on " << nStates
                                            << " filtered track states.");
       }
+
       // Smooth the track states
       auto smoothRes = m_smoother(state.geoContext, result.fittedStates,
                                   measurementIndices.front());
-
       if (!smoothRes.ok()) {
         ACTS_ERROR("Smoothing step failed: " << smoothRes.error());
         return smoothRes.error();

Core/include/Acts/Fitter/detail/KalmanGlobalCovariance.hpp

@@ -0,0 +1,108 @@
+// This file is part of the Acts project.
+//
+// Copyright (C) 2020 CERN for the benefit of the Acts project
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include "Acts/EventData/MultiTrajectory.hpp"
+#include "Acts/EventData/TrackParameters.hpp"
+#include "Acts/Geometry/GeometryContext.hpp"
+#include "Acts/Surfaces/Surface.hpp"
+#include "Acts/Utilities/Definitions.hpp"
+#include "Acts/Utilities/ParameterDefinitions.hpp"
+
+#include <unordered_map>
+
+namespace Acts {
+namespace detail {
+/// Calculate the global track parameters covariance for a smoothed trajectory
+/// stored in MultiTrajecty based on formulas at Journal of Physics: Conference
+/// Series 219 (2010) 032028.
+///
+/// @tparam source_link_t The source link type of the trajectory
+/// @tparam parameters_t The track parameters type
+///
+/// @param multiTraj The MultiTrajectory containing the trajectory to be
+/// investigated
+/// @param entryIndex The trajectory entry index
+///
+/// @return The global track parameters covariance matrix and the starting
+/// row/column for smoothed states
+template <typename source_link_t, typename parameters_t = BoundParameters>
+std::pair<ActsMatrixX<BoundParametersScalar>,
+          std::unordered_map<size_t, size_t>>
+globalTrackParametersCovariance(
+    const Acts::MultiTrajectory<source_link_t>& multiTraj,
+    const size_t& entryIndex) {
+  using CovMatrix = typename parameters_t::CovMatrix_t;
+  using GainMatrix = CovMatrix;
+
+  // The last smoothed state index
+  size_t lastSmoothedIndex = SIZE_MAX;
+  // The total number of smoothed states
+  size_t nSmoothedStates = 0;
+  // Visit all the states
+  multiTraj.visitBackwards(entryIndex, [&](const auto& ts) {
+    if (ts.hasSmoothed()) {
+      if (lastSmoothedIndex == SIZE_MAX) {
+        lastSmoothedIndex = ts.index();
+      }
+      nSmoothedStates++;
+    }
+  });
+
+  // Set the size of global track parameters covariance for all smoothed states
+  ActsMatrixX<BoundParametersScalar> fullGlobalTrackParamsCov =
+      ActsMatrixX<BoundParametersScalar>::Zero(
+          nSmoothedStates * eBoundParametersSize,
+          nSmoothedStates * eBoundParametersSize);
+  // The index of state within the trajectory and the starting row/column for
+  // this state in the global covariance matrix
+  std::unordered_map<size_t, size_t> stateRowIndices;
+  // Visit the smoothed states to calculate the full global track parameters
+  // covariance
+  size_t nProcessed = 0;
+  auto prev_ts = multiTraj.getTrackState(lastSmoothedIndex);
+  multiTraj.visitBackwards(lastSmoothedIndex, [&](const auto& ts) {
+    const size_t iRow = fullGlobalTrackParamsCov.rows() -
+                        eBoundParametersSize * (nProcessed + 1);
+    // Fill the covariance of this state
+    fullGlobalTrackParamsCov.block<eBoundParametersSize, eBoundParametersSize>(
+        iRow, iRow) = ts.smoothedCovariance();
+    // Fill the correlation between this state (indexed by i-1) and
+    // beforehand smoothed states (indexed by j): C^n_{i-1, j}= G_{i-1} *
+    // C^n_{i, j} for i <= j
+    if (nProcessed > 0) {
+      // Calculate the gain matrix
+      GainMatrix G = ts.filteredCovariance() * prev_ts.jacobian().transpose() *
+                     prev_ts.predictedCovariance().inverse();
+      // Loop over the beforehand smoothed states
+      for (size_t iProcessed = 1; iProcessed <= nProcessed; iProcessed++) {
+        const size_t iCol = iRow + eBoundParametersSize * iProcessed;
+        CovMatrix prev_correlation =
+            fullGlobalTrackParamsCov
+                .block<eBoundParametersSize, eBoundParametersSize>(
+                    iRow + eBoundParametersSize, iCol);
+        CovMatrix correlation = G * prev_correlation;
+        fullGlobalTrackParamsCov
+            .block<eBoundParametersSize, eBoundParametersSize>(iRow, iCol) =
+            correlation;
+        fullGlobalTrackParamsCov
+            .block<eBoundParametersSize, eBoundParametersSize>(iCol, iRow) =
+            correlation.transpose();
+      }
+    }
+    stateRowIndices.emplace(ts.index(), iRow);
+    nProcessed++;
+    prev_ts = ts;
+  });
+
+  return std::make_pair(fullGlobalTrackParamsCov, stateRowIndices);
+}
+
+}  // namespace detail
+}  // namespace Acts

Core/include/Acts/Surfaces/ConeSurface.hpp

@@ -212,6 +212,17 @@ class ConeSurface : public Surface {
   /// Return properly formatted class name for screen output
   std::string name() const override;
 
+  /// Calculate the derivative of bound track parameters local position w.r.t.
+  /// position in local 3D Cartesian coordinates
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param position The position of the paramters in global
+  ///
+  /// @return Derivative of bound local position w.r.t. position in local 3D
+  /// cartesian coordinates
+  const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative(
+      const GeometryContext& gctx, const Vector3D& position) const final;
+
  protected:
   std::shared_ptr<const ConeBounds> m_bounds;  ///< bounds (shared)
 

Core/include/Acts/Surfaces/CylinderSurface.hpp

@@ -219,6 +219,17 @@ class CylinderSurface : public Surface {
   Polyhedron polyhedronRepresentation(const GeometryContext& gctx,
                                       size_t lseg) const override;
 
+  /// Calculate the derivative of bound track parameters local position w.r.t.
+  /// position in local 3D Cartesian coordinates
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param position The position of the paramters in global
+  ///
+  /// @return Derivative of bound local position w.r.t. position in local 3D
+  /// cartesian coordinates
+  const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative(
+      const GeometryContext& gctx, const Vector3D& position) const final;
+
  protected:
   std::shared_ptr<const CylinderBounds> m_bounds;  //!< bounds (shared)
 

Core/include/Acts/Surfaces/DiscSurface.hpp

@@ -310,6 +310,17 @@ class DiscSurface : public Surface {
   Polyhedron polyhedronRepresentation(const GeometryContext& gctx,
                                       size_t lseg) const override;
 
+  /// Calculate the derivative of bound track parameters local position w.r.t.
+  /// position in local 3D Cartesian coordinates
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param position The position of the paramters in global
+  ///
+  /// @return Derivative of bound local position w.r.t. position in local 3D
+  /// cartesian coordinates
+  const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative(
+      const GeometryContext& gctx, const Vector3D& position) const final;
+
  protected:
   std::shared_ptr<const DiscBounds> m_bounds;  ///< bounds (shared)
 };

Core/include/Acts/Surfaces/LineSurface.hpp

@@ -257,6 +257,32 @@ class LineSurface : public Surface {
   /// Return properly formatted class name for screen output */
   std::string name() const override;
 
+  /// Calculate the derivative of path length w.r.t. alignment parameters of the
+  /// surface (i.e. local frame origin in global 3D Cartesian coordinates and
+  /// its rotation represented with extrinsic Euler angles)
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param rotToLocalZAxis The derivative of local frame z axis vector w.r.t.
+  /// its rotation
+  /// @param position The position of the paramters in global
+  /// @param direction The direction of the track
+  ///
+  /// @return Derivative of path length w.r.t. the alignment parameters
+  const AlignmentRowVector alignmentToPathDerivative(
+      const GeometryContext& gctx, const RotationMatrix3D& rotToLocalZAxis,
+      const Vector3D& position, const Vector3D& direction) const final;
+
+  /// Calculate the derivative of bound track parameters local position w.r.t.
+  /// position in local 3D Cartesian coordinates
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param position The position of the paramters in global
+  ///
+  /// @return Derivative of bound local position w.r.t. position in local 3D
+  /// cartesian coordinates
+  const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative(
+      const GeometryContext& gctx, const Vector3D& position) const final;
+
  protected:
   std::shared_ptr<const LineBounds> m_bounds;  ///< bounds (shared)
 

Core/include/Acts/Surfaces/PlaneSurface.hpp

@@ -195,6 +195,17 @@ class PlaneSurface : public Surface {
   /// Return properly formatted class name for screen output
   std::string name() const override;
 
+  /// Calculate the derivative of bound track parameters local position w.r.t.
+  /// position in local 3D Cartesian coordinates
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param position The position of the paramters in global
+  ///
+  /// @return Derivative of bound local position w.r.t. position in local 3D
+  /// cartesian coordinates
+  const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative(
+      const GeometryContext& gctx, const Vector3D& position) const final;
+
  protected:
   /// the bounds of this surface
   std::shared_ptr<const PlanarBounds> m_bounds;

Core/include/Acts/Surfaces/Surface.hpp

@@ -15,6 +15,8 @@
 #include "Acts/Geometry/Polyhedron.hpp"
 #include "Acts/Surfaces/BoundaryCheck.hpp"
 #include "Acts/Surfaces/SurfaceBounds.hpp"
+#include "Acts/Surfaces/detail/AlignmentHelper.hpp"
+#include "Acts/Utilities/AlignmentDefinitions.hpp"
 #include "Acts/Utilities/BinnedArray.hpp"
 #include "Acts/Utilities/BinningType.hpp"
 #include "Acts/Utilities/Definitions.hpp"
@@ -258,6 +260,24 @@ class Surface : public virtual GeometryObject,
                    const Vector3D& momentum,
                    const BoundaryCheck& bcheck = true) const;
 
+  /// The derivative of bound track parameters w.r.t. alignment
+  /// parameters of its reference surface (i.e. local frame origin in
+  /// global 3D Cartesian coordinates and its rotation represented with
+  /// extrinsic Euler angles)
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param derivatives Path length derivatives of the free, nominal
+  /// parameters to help evaluate change of free track parameters caused by
+  /// change of alignment parameters
+  /// @param position The position of the paramters in global
+  /// @param direction The direction of the track
+  ///
+  /// @return Derivative of bound track parameters w.r.t. local frame
+  /// alignment parameters
+  const AlignmentToBoundMatrix alignmentToBoundDerivative(
+      const GeometryContext& gctx, const FreeVector& derivatives,
+      const Vector3D& position, const Vector3D& direction) const;
+
   /// The insideBounds method for local positions
   ///
   /// @param lposition The local position to check
@@ -446,6 +466,36 @@ class Surface : public virtual GeometryObject,
   virtual Polyhedron polyhedronRepresentation(const GeometryContext& gctx,
                                               size_t lseg) const = 0;
 
+  /// Calculate the derivative of path length w.r.t. alignment parameters of the
+  /// surface (i.e. local frame origin in global 3D Cartesian coordinates and
+  /// its rotation represented with extrinsic Euler angles)
+  ///
+  /// Re-implementation is needed for surface whose intersection with track is
+  /// not its local xy plane, e.g. LineSurface, CylinderSurface and ConeSurface
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param rotToLocalZAxis The derivative of local frame z axis vector w.r.t.
+  /// its rotation
+  /// @param position The position of the paramters in global
+  /// @param direction The direction of the track
+  ///
+  /// @return Derivative of path length w.r.t. the alignment parameters
+  virtual const AlignmentRowVector alignmentToPathDerivative(
+      const GeometryContext& gctx, const RotationMatrix3D& rotToLocalZAxis,
+      const Vector3D& position, const Vector3D& direction) const;
+
+  /// Calculate the derivative of bound track parameters local position w.r.t.
+  /// position in local 3D Cartesian coordinates
+  ///
+  /// @param gctx The current geometry context object, e.g. alignment
+  /// @param position The position of the paramters in global
+  ///
+  /// @return Derivative of bound local position w.r.t. position in local 3D
+  /// cartesian coordinates
+  virtual const LocalCartesianToBoundLocalMatrix
+  localCartesianToBoundLocalDerivative(const GeometryContext& gctx,
+                                       const Vector3D& position) const = 0;
+
  protected:
   /// Transform3D definition that positions
   /// (translation, rotation) the surface in global space

Core/include/Acts/Surfaces/detail/AlignmentHelper.hpp

@@ -0,0 +1,37 @@
+// This file is part of the Acts project.
+//
+// Copyright (C) 2020 CERN for the benefit of the Acts project
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include <utility>
+#include <vector>
+#include "Acts/Utilities/Definitions.hpp"
+
+namespace Acts {
+
+namespace detail {
+
+// The container for derivative of local frame axis w.r.t. its
+// rotation parameters. The first element is for x axis, second for y axis and
+// last for z axis
+using RotationToAxes =
+    std::tuple<RotationMatrix3D, RotationMatrix3D, RotationMatrix3D>;
+
+/// @brief Evaluate the derivative of local frame axes vector w.r.t.
+/// its rotation around global x/y/z axis
+/// @Todo: add parameter for rotation axis order
+///
+/// @param rotation The rotation that help place the surface
+///
+/// @return Derivative of local frame x/y/z axis vector w.r.t. its
+/// rotation angles (extrinsic Euler angles) around global x/y/z axis
+RotationToAxes rotationToLocalAxesDerivative(const RotationMatrix3D& rotation);
+
+}  // namespace detail
+
+}  // namespace Acts