Implement new TrackState EDM used in KF

This MR adds a new track state EDM called MultiTrajectory. It acts as a track state collection, which internally stores all track state components in arrays. Each track state is then an index tuple, pointing to the components in the backing storage. Each track state has a previous index. Iterating back from a track state over these previous indices until you reach the beginning of the trajectory gives you a consistent set of track state. In the case of the regular KF, this is always a simple sequence, but it can be used to have forking trajectories, that can then be traced back individually.

Additionally, the index structure allows reusing storage between track state. If you have a track state with one set of predicted parameters, for instance, it will allow you to clone a track state, only resetting some components, and reusing shared ones (this bit isn't actually implemented yet).

Measurements are stored in an overallocated way, meaning there is always storage for the maximum measurement dimension.

MultiTrajectory can be queried for a track state representation at given index. This will return a TrackStateProxy. Track state proxies have getters for all components, and where possible, will return mutable (or immutable in case of a const proxy) references (sometimes implemented as Eigen map types). This means you can write things like

auto trackstate = trajectory.getTrackState(4);
trackstate.filtered() = (A * B + C).inverse()

and the result will end up in the backing storage of MultiTrajectory. Methods for the calibrated measurement are implemented as both returning the actual overallocated backing storage matrix, as well as an effective matrix. The latter are in fact dynamic views into the static matrices, and thus should be avoided in any kind of context where performance is relevant.

The new EDM is used in the updated gain matrix updator and smoother. Here, the measurement visitor (see !598 (merged)), to dispatch a statically sized calculation based on the runtime measurement value that is stored in the trajectory.

This MR requires !598 (merged) and !596 (merged) to be merged first (i.e. includes their changes)

Closes #564 (closed) Closes #550 (closed)

Core/include/Acts/EventData/LocalTrajectory.hpp

@@ -48,26 +48,35 @@ namespace detail_lt {
     }
   };
   /// Type construction helper for coefficients and associated covariances.
-  ///
-  /// All types have an upper size bound but the
   template <Eigen::Index MaxSize, Eigen::Index SizeIncrement = 8>
   struct Types
   {
     static constexpr int Flags = Eigen::ColMajor | Eigen::AutoAlign;
     using Scalar               = double;
+
+    // storage of multiple items in flat arrays (with overallocation up to max)
     using StorageCoefficients
         = GrowableColumns<Eigen::Array<Scalar, MaxSize, Eigen::Dynamic, Flags>,
                           SizeIncrement>;
     using StorageCovariance = GrowableColumns<
         Eigen::Array<Scalar, MaxSize * MaxSize, Eigen::Dynamic, Flags>,
         SizeIncrement>;
+
+    // full single items
     using FullCoefficients = Eigen::Matrix<Scalar, MaxSize, 1, Flags>;
     using FullCovariance   = Eigen::Matrix<Scalar, MaxSize, MaxSize, Flags>;
+    using FullCoefficientsConstMap = Eigen::Map<const FullCoefficients>;
+    using FullCovarianceConstMap   = Eigen::Map<const FullCovariance>;
+
+    // sub-vector, sub-matrix single items
     using Coefficients
         = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Flags, MaxSize, 1>;
     using Covariance = Eigen::
         Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Flags, MaxSize, MaxSize>;
-    using CovarianceStride = Eigen::Stride<MaxSize, 1>;
+    using CoefficientsConstMap = Eigen::Map<const Coefficients>;
+    // TODO can we determine the pointer alignment at compile time?
+    using CovarianceConstMap = Eigen::
+        Map<const Covariance, Eigen::Unaligned, Eigen::Stride<MaxSize, 1>>;
   };
   struct PointData
   {
@@ -86,28 +95,41 @@ class LocalTrajectoryPoint
 {
 public:
   // underlying storage types w/ overallocation
-  using FullParameters        = detail_lt::Types<8>::FullCoefficients;
-  using FullCovariance        = detail_lt::Types<8>::FullCovariance;
-  using Parameters            = detail_lt::Types<8>::Coefficients;
-  using Covariance            = detail_lt::Types<8>::Covariance;
-  using CovarianceStride      = detail_lt::Types<8>::CovarianceStride;
-  using Measurement           = detail_lt::Types<2>::Coefficients;
-  using MeasurementCovariance = detail_lt::Types<2>::Covariance;
+  using FullParameters  = detail_lt::Types<8>::FullCoefficientsConstMap;
+  using FullCovariance  = detail_lt::Types<8>::FullCovarianceConstMap;
+  using FullMeasurement = detail_lt::Types<2>::FullCoefficientsConstMap;
+  using FullMeasurementCovariance = detail_lt::Types<2>::FullCovarianceConstMap;
+  using Parameters                = detail_lt::Types<8>::CoefficientsConstMap;
+  using Covariance                = detail_lt::Types<8>::CovarianceConstMap;
+  using Measurement               = detail_lt::Types<2>::CoefficientsConstMap;
+  using MeasurementCovariance     = detail_lt::Types<2>::CovarianceConstMap;
 
   LocalTrajectoryPoint(const LocalTrajectory& trajectory, size_t index);
 
-  Eigen::Map<const FullParameters>
+  FullParameters
   fullParameters() const;
-  Eigen::Map<const FullCovariance>
+  FullCovariance
   fullCovariance() const;
-  Eigen::Map<const Parameters>
+  FullMeasurement
+  fullMeasurement() const;
+  FullMeasurementCovariance
+  fullMeasurementCovariance() const;
+
+  Parameters
   parameters() const;
-  Eigen::Map<const Covariance, Eigen::Unaligned, CovarianceStride>
+  Covariance
   covariance() const;
+  Measurement
+  measurement() const;
+  MeasurementCovariance
+  measurementCovariance() const;
 
   /// Check if the point has an associated measurement.
   bool
-  hasMeasurement() const;
+  hasMeasurement() const
+  {
+    return m_point.imeas != decltype(m_point)::kInvalid;
+  }
 
 private:
   const LocalTrajectory& m_traj;
@@ -150,29 +172,41 @@ inline LocalTrajectoryPoint::LocalTrajectoryPoint(
 {
 }
 
-inline Eigen::Map<const LocalTrajectoryPoint::FullParameters>
+inline LocalTrajectoryPoint::FullParameters
 LocalTrajectoryPoint::fullParameters() const
 {
-  return Eigen::Map<const LocalTrajectoryPoint::FullParameters>(
+  return LocalTrajectoryPoint::FullParameters(
       m_traj.m_params.data.col(m_point.iparams).data());
 }
 
-inline Eigen::Map<const LocalTrajectoryPoint::FullCovariance>
+inline LocalTrajectoryPoint::FullCovariance
 LocalTrajectoryPoint::fullCovariance() const
 {
-  return Eigen::Map<const LocalTrajectoryPoint::FullCovariance>(
+  return LocalTrajectoryPoint::FullCovariance(
       m_traj.m_cov.data.col(m_point.iparams).data());
 }
 
-inline Eigen::Map<const LocalTrajectoryPoint::Parameters>
+inline LocalTrajectoryPoint::FullMeasurement
+LocalTrajectoryPoint::fullMeasurement() const
+{
+  return LocalTrajectoryPoint::FullMeasurement(
+      m_traj.m_meas.data.col(m_point.imeas).data());
+}
+
+inline LocalTrajectoryPoint::FullMeasurementCovariance
+LocalTrajectoryPoint::fullMeasurementCovariance() const
+{
+  return LocalTrajectoryPoint::FullMeasurementCovariance(
+      m_traj.m_measCov.data.col(m_point.imeas).data());
+}
+
+inline LocalTrajectoryPoint::Parameters
 LocalTrajectoryPoint::parameters() const
 {
   return {m_traj.m_params.data.col(m_point.iparams).data(), m_point.nparams};
 }
 
-inline Eigen::Map<const LocalTrajectoryPoint::Covariance,
-                  Eigen::Unaligned,
-                  LocalTrajectoryPoint::CovarianceStride>
+inline LocalTrajectoryPoint::Covariance
 LocalTrajectoryPoint::covariance() const
 {
   return {m_traj.m_cov.data.col(m_point.iparams).data(),
@@ -180,10 +214,18 @@ LocalTrajectoryPoint::covariance() const
           m_point.nparams};
 }
 
-inline bool
-LocalTrajectoryPoint::hasMeasurement() const
+inline LocalTrajectoryPoint::Measurement
+LocalTrajectoryPoint::measurement() const
+{
+  return {m_traj.m_meas.data.col(m_point.imeas).data(), m_point.nmeas};
+}
+
+inline LocalTrajectoryPoint::MeasurementCovariance
+LocalTrajectoryPoint::measurementCovariance() const
 {
-  return m_point.imeas != decltype(m_point)::kInvalid;
+  return {m_traj.m_measCov.data.col(m_point.imeas).data(),
+          m_point.nmeas,
+          m_point.nmeas};
 }
 
 inline size_t