refactor: Allow TrackState measurement setting without extra initiali…

…zation (#3907) Previously, `VectorMultiTrajectory` would initialize measurement values and covariances to zero. This has some amount of overhead. With this 1. Calling `allocateCalibrated` on a track state with an existing allocation of different size is now an error 2. An extra overload to `allocateCalibrated` now accepts arguments for the value and covariance that it will initialize the allocation to. 3. The previous signature remains valid, and will initialize the allocation to zero.  ## Summary by CodeRabbit - **New Features** - Introduced new methods for allocating and managing calibrated measurements across various classes. - Added a `size()` method to `VectorTrackContainer` for easy retrieval of track count. - Enhanced `CombinatorialKalmanFilter` with a new `findTracks` method for improved track finding capabilities. - Added a new `MultiTrajectoryAllocateCalibratedInit` test case to validate the allocation of calibrated initial states. - **Bug Fixes** - Improved error handling in the `findTracks` method to log propagation errors. - Updated memory management in `VectorMultiTrajectory` to ensure proper state invalidation. - **Tests** - Added new test cases for validating the allocation and initialization of calibrated states. - Modified existing tests to reflect changes in the handling of calibrated measurements.
acts-project · Nov 26, 2024 · 7caafd7 · 7caafd7
1 parent 622902b
commit 7caafd7
Show file tree

Hide file tree

Showing 20 changed files with 253 additions and 61 deletions.
diff --git a/Core/include/Acts/EventData/MultiTrajectory.hpp b/Core/include/Acts/EventData/MultiTrajectory.hpp
@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include "Acts/Definitions/Algebra.hpp"
 #include "Acts/Definitions/TrackParametrization.hpp"
 #include "Acts/EventData/MeasurementHelpers.hpp"
 #include "Acts/EventData/SourceLink.hpp"
@@ -687,13 +688,24 @@ class MultiTrajectory {
   /// Allocate storage for a calibrated measurement of specified dimension
   /// @param istate The track state to store for
   /// @param measdim the dimension of the measurement to store
-  /// @note Is a noop if the track state already has an allocation
-  ///       an the dimension is the same.
+  /// @note In case an allocation is already present, no additional allocation
+  ///       will be performed, but the existing allocation will be zeroed.
   void allocateCalibrated(IndexType istate, std::size_t measdim) {
     throw_assert(measdim > 0 && measdim <= eBoundSize,
                  "Invalid measurement dimension detected");
 
-    self().allocateCalibrated_impl(istate, measdim);
+    visit_measurement(measdim, [this, istate]<std::size_t DIM>(
+                                   std::integral_constant<std::size_t, DIM>) {
+      self().template allocateCalibrated_impl(
+          istate, ActsVector<DIM>{ActsVector<DIM>::Zero()},
+          ActsSquareMatrix<DIM>{ActsSquareMatrix<DIM>::Zero()});
+    });
+  }
+
+  template <std::size_t measdim, typename val_t, typename cov_t>
+  void allocateCalibrated(IndexType istate, const Eigen::DenseBase<val_t>& val,
+                          const Eigen::DenseBase<cov_t>& cov) {
+    self().template allocateCalibrated_impl(istate, val, cov);
   }
 
   void setUncalibratedSourceLink(IndexType istate, SourceLink&& sourceLink)

diff --git a/Core/include/Acts/EventData/MultiTrajectoryBackendConcept.hpp b/Core/include/Acts/EventData/MultiTrajectoryBackendConcept.hpp
@@ -130,7 +130,15 @@ concept MutableMultiTrajectoryBackend =
       { v.template addColumn_impl<float>(col) };
       { v.template addColumn_impl<double>(col) };
 
-      { v.allocateCalibrated_impl(istate, dim) };
+      {
+        v.template allocateCalibrated_impl(istate, ActsVector<1>{},
+                                           ActsSquareMatrix<1>{})
+      };
+      // Assuming intermediate values also work
+      {
+        v.template allocateCalibrated_impl(istate, ActsVector<eBoundSize>{},
+                                           ActsSquareMatrix<eBoundSize>{})
+      };
 
       { v.setUncalibratedSourceLink_impl(istate, std::move(sl)) };
 

diff --git a/Core/include/Acts/EventData/TrackProxy.hpp b/Core/include/Acts/EventData/TrackProxy.hpp
@@ -577,9 +577,6 @@ class TrackProxy {
       // append track states (cheap), but they're in the wrong order
       for (const auto& srcTrackState : other.trackStatesReversed()) {
         auto destTrackState = appendTrackState(srcTrackState.getMask());
-        if (srcTrackState.hasCalibrated()) {
-          destTrackState.allocateCalibrated(srcTrackState.calibratedSize());
-        }
         destTrackState.copyFrom(srcTrackState, Acts::TrackStatePropMask::All,
                                 true);
       }

diff --git a/Core/include/Acts/EventData/TrackStateProxy.hpp b/Core/include/Acts/EventData/TrackStateProxy.hpp
@@ -865,10 +865,35 @@ class TrackStateProxy {
 
   /// Allocate storage to be able to store a measurement of size @p measdim.
   /// This must be called **before** setting the measurement content.
+  /// @note This does not allocate if an allocation of the same size already exists
+  /// @note This will zero-initialize the allocated storage
+  /// @note This is an error if an existing allocation has different size
   void allocateCalibrated(std::size_t measdim) {
     m_traj->allocateCalibrated(m_istate, measdim);
   }
 
+  /// Allocate storage and assign the given vector and covariance to it.
+  /// The dimension is inferred from the given vector and matrix.
+  /// @tparam val_t Type of the vector
+  /// @tparam cov_t Type of the covariance matrix
+  /// @param val The measurement vector
+  /// @param cov The covariance matrix
+  /// @note This does not allocate if an allocation of the same size already exists
+  /// @note This throws an exception if an existing allocation has different size
+  template <typename val_t, typename cov_t>
+  void allocateCalibrated(const Eigen::DenseBase<val_t>& val,
+                          const Eigen::DenseBase<cov_t>& cov)
+    requires(Eigen::PlainObjectBase<val_t>::RowsAtCompileTime > 0 &&
+             Eigen::PlainObjectBase<val_t>::RowsAtCompileTime <= eBoundSize &&
+             Eigen::PlainObjectBase<val_t>::RowsAtCompileTime ==
+                 Eigen::PlainObjectBase<cov_t>::RowsAtCompileTime &&
+             Eigen::PlainObjectBase<cov_t>::RowsAtCompileTime ==
+                 Eigen::PlainObjectBase<cov_t>::ColsAtCompileTime)
+  {
+    m_traj->template allocateCalibrated<
+        Eigen::PlainObjectBase<val_t>::RowsAtCompileTime>(m_istate, val, cov);
+  }
+
   /// @}
 
   /// @anchor track_state_share_copy
@@ -993,17 +1018,14 @@ class TrackStateProxy {
       }
 
       if (ACTS_CHECK_BIT(src, PM::Calibrated)) {
-        allocateCalibrated(other.calibratedSize());
-
         // workaround for gcc8 bug:
         // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=86594
         auto* self = this;
         visit_measurement(other.calibratedSize(), [&](auto N) {
           constexpr int measdim = decltype(N)::value;
-          self->template calibrated<measdim>() =
-              other.template calibrated<measdim>();
-          self->template calibratedCovariance<measdim>() =
-              other.template calibratedCovariance<measdim>();
+          self->allocateCalibrated(
+              other.template calibrated<measdim>().eval(),
+              other.template calibratedCovariance<measdim>().eval());
         });
 
         setBoundSubspaceIndices(other.boundSubspaceIndices());
@@ -1041,17 +1063,14 @@ class TrackStateProxy {
       // may be not yet allocated
       if (ACTS_CHECK_BIT(mask, PM::Calibrated) &&
           other.template has<hashString("calibrated")>()) {
-        allocateCalibrated(other.calibratedSize());
-
         // workaround for gcc8 bug:
         // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=86594
         auto* self = this;
         visit_measurement(other.calibratedSize(), [&](auto N) {
           constexpr int measdim = decltype(N)::value;
-          self->template calibrated<measdim>() =
-              other.template calibrated<measdim>();
-          self->template calibratedCovariance<measdim>() =
-              other.template calibratedCovariance<measdim>();
+          self->allocateCalibrated(
+              other.template calibrated<measdim>().eval(),
+              other.template calibratedCovariance<measdim>().eval());
         });
 
         setBoundSubspaceIndices(other.boundSubspaceIndices());

diff --git a/Core/include/Acts/EventData/TrackStateProxyConcept.hpp b/Core/include/Acts/EventData/TrackStateProxyConcept.hpp
@@ -248,6 +248,13 @@ concept MutableTrackStateProxyConcept =
 
       { v.allocateCalibrated(measdim) };
 
+      { v.allocateCalibrated(ActsVector<1>{}, ActsSquareMatrix<1>{}) };
+      // Assuming intermediate values are also allowed
+      {
+        v.allocateCalibrated(ActsVector<eBoundSize>{},
+                             ActsSquareMatrix<eBoundSize>{})
+      };
+
       { v.chi2() } -> std::same_as<double&>;
 
       { v.pathLength() } -> std::same_as<double&>;

diff --git a/Core/include/Acts/EventData/VectorMultiTrajectory.hpp b/Core/include/Acts/EventData/VectorMultiTrajectory.hpp
@@ -47,6 +47,33 @@ using MultiTrajectoryTraits::IndexType;
 constexpr auto kInvalid = MultiTrajectoryTraits::kInvalid;
 constexpr auto MeasurementSizeMax = MultiTrajectoryTraits::MeasurementSizeMax;
 
+template <typename T>
+struct NonInitializingAllocator {
+  using value_type = T;
+
+  NonInitializingAllocator() noexcept = default;
+
+  template <class U>
+  explicit NonInitializingAllocator(
+      const NonInitializingAllocator<U>& /*other*/) noexcept {}
+
+  template <class U>
+  bool operator==(const NonInitializingAllocator<U>& /*other*/) const noexcept {
+    return true;
+  }
+
+  T* allocate(std::size_t n) const { return std::allocator<T>{}.allocate(n); }
+
+  void deallocate(T* const p, std::size_t n) const noexcept {
+    std::allocator<T>{}.deallocate(p, n);
+  }
+
+  void construct(T* /*p*/) const {
+    // This construct function intentionally does not initialize the object!
+    // Be very careful when using this allocator.
+  }
+};
+
 class VectorMultiTrajectoryBase {
  public:
   struct Statistics {
@@ -320,9 +347,9 @@ class VectorMultiTrajectoryBase {
       m_params;
   std::vector<typename detail_lt::FixedSizeTypes<eBoundSize>::Covariance> m_cov;
 
-  std::vector<double> m_meas;
+  std::vector<double, NonInitializingAllocator<double>> m_meas;
   std::vector<MultiTrajectoryTraits::IndexType> m_measOffset;
-  std::vector<double> m_measCov;
+  std::vector<double, NonInitializingAllocator<double>> m_measCov;
   std::vector<MultiTrajectoryTraits::IndexType> m_measCovOffset;
 
   std::vector<typename detail_lt::FixedSizeTypes<eBoundSize>::Covariance> m_jac;
@@ -464,23 +491,44 @@ class VectorMultiTrajectory final
     return detail_vmt::VectorMultiTrajectoryBase::hasColumn_impl(*this, key);
   }
 
-  void allocateCalibrated_impl(IndexType istate, std::size_t measdim) {
-    throw_assert(measdim > 0 && measdim <= eBoundSize,
-                 "Invalid measurement dimension detected");
+  template <typename val_t, typename cov_t>
+  void allocateCalibrated_impl(IndexType istate,
+                               const Eigen::DenseBase<val_t>& val,
+                               const Eigen::DenseBase<cov_t>& cov)
+
+    requires(Eigen::PlainObjectBase<val_t>::RowsAtCompileTime > 0 &&
+             Eigen::PlainObjectBase<val_t>::RowsAtCompileTime <= eBoundSize &&
+             Eigen::PlainObjectBase<val_t>::RowsAtCompileTime ==
+                 Eigen::PlainObjectBase<cov_t>::RowsAtCompileTime &&
+             Eigen::PlainObjectBase<cov_t>::RowsAtCompileTime ==
+                 Eigen::PlainObjectBase<cov_t>::ColsAtCompileTime)
+  {
+    constexpr std::size_t measdim = val_t::RowsAtCompileTime;
+
+    if (m_index[istate].measdim != kInvalid &&
+        m_index[istate].measdim != measdim) {
+      throw std::invalid_argument{
+          "Measurement dimension does not match the allocated dimension"};
+    }
+
+    if (m_measOffset[istate] == kInvalid ||
+        m_measCovOffset[istate] == kInvalid) {
+      m_measOffset[istate] = static_cast<IndexType>(m_meas.size());
+      m_meas.resize(m_meas.size() + measdim);
 
-    if (m_measOffset[istate] != kInvalid &&
-        m_measCovOffset[istate] != kInvalid &&
-        m_index[istate].measdim == measdim) {
-      return;
+      m_measCovOffset[istate] = static_cast<IndexType>(m_measCov.size());
+      m_measCov.resize(m_measCov.size() + measdim * measdim);
     }
 
     m_index[istate].measdim = measdim;
 
-    m_measOffset[istate] = static_cast<IndexType>(m_meas.size());
-    m_meas.resize(m_meas.size() + measdim);
+    double* measPtr = &m_meas[m_measOffset[istate]];
+    Eigen::Map<ActsVector<measdim>> valMap(measPtr);
+    valMap = val;
 
-    m_measCovOffset[istate] = static_cast<IndexType>(m_measCov.size());
-    m_measCov.resize(m_measCov.size() + measdim * measdim);
+    double* covPtr = &m_measCov[m_measCovOffset[istate]];
+    Eigen::Map<ActsSquareMatrix<measdim>> covMap(covPtr);
+    covMap = cov;
   }
 
   void setUncalibratedSourceLink_impl(IndexType istate,

diff --git a/Core/include/Acts/EventData/VectorTrackContainer.hpp b/Core/include/Acts/EventData/VectorTrackContainer.hpp
@@ -253,6 +253,7 @@ class VectorTrackContainer final : public detail_vtc::VectorTrackContainerBase {
 
   void reserve(IndexType size);
   void clear();
+  std::size_t size() const;
 
   void setReferenceSurface_impl(IndexType itrack,
                                 std::shared_ptr<const Surface> surface) {