Line data    Source code

       1              : #ifndef D118B69B_C3E7_43F7_A2FC_F44B7ACF965F
       2              : #define D118B69B_C3E7_43F7_A2FC_F44B7ACF965F
       3              : 
       4              : #include "base/first_include.h"                                  // IWYU pragma: keep
       5              : #include "math/linalg/contracts/covariance_matrix_policy_intf.h" // IWYU pragma: keep
       6              : #include "math/linalg/covariance_matrix_policies.h"              // IWYU pragma: keep
       7              : #include "math/linalg/vector.h"
       8              : #include "motion/contracts/state_mem_intf.h"
       9              : 
      10              : namespace tracking
      11              : {
      12              : namespace motion
      13              : {
      14              : 
      15              : // TODO(matthias): replace dynamic memory allocation by static allocator https://godbolt.org/z/qb4jxsEYM
      16              : // TODO(matthias): or introduce CTORs from all other motion models to allow conversion
      17              : 
      18              : /// \brief StateMem class to represent a state vector with its uncertainty described by the state covariance matrix
      19              : /// \tparam CovarianceMatrixPolicy_ Policy type that defines the covariance matrix implementation
      20              : /// \tparam Size_                   State dimension
      21              : template <typename CovarianceMatrixPolicy_, sint32 Size_>
      22              : class StateMem
      23              :     : public contract::StateMemIntf<StateMem<CovarianceMatrixPolicy_, Size_>>
      24              :     , public math::contract::CovarianceMatrixPolicyIntf<CovarianceMatrixPolicy_>
      25              : {
      26              : public:
      27              :   using value_type    = typename CovarianceMatrixPolicy_::value_type;
      28              :   using StateVec      = math::Vector<value_type, Size_>;
      29              :   using ConstStateVec = const math::Vector<value_type, Size_>;
      30              :   using StateCov      = typename CovarianceMatrixPolicy_::template Instantiate<Size_>;
      31              :   using ConstStateCov = const StateCov;
      32              : 
      33              :   // rule of 5 declarations
      34            4 :   StateMem()                                         = default;
      35              :   StateMem(const StateMem& other)                    = default;
      36              :   StateMem(StateMem&&) noexcept                      = default;
      37              :   auto operator=(const StateMem& other) -> StateMem& = default;
      38              :   auto operator=(StateMem&&) noexcept -> StateMem&   = default;
      39           12 :   virtual ~StateMem()                                = default;
      40              : 
      41              :   /// \brief Read access to full state vector
      42              :   /// \return const StateVec&
      43            4 :   auto getVec() const -> ConstStateVec& { return _vec; }
      44              : 
      45              :   /// \brief Read/Write access to state vector
      46              :   /// \return StateVec&
      47              :   /// \note Only for INTERNAL usage! Required in generic::Predict<..>::run().
      48           80 :   auto getVecForInternalUse() -> StateVec& { return _vec; }
      49              : 
      50              :   /// \brief Read access to state covariance matrix
      51              :   /// \return const StateCov&
      52            4 :   auto getCov() const -> ConstStateCov& { return _cov; }
      53              : 
      54              :   /// \brief Read/Write access to state covariance matrix
      55              :   /// \return StateCov&
      56              :   /// \note Only for INTERNAL usage! Required in generic::Predict<..>::run().
      57          160 :   auto getCovForInternalUse() -> StateCov& { return _cov; }
      58              : 
      59              :   /// \brief Read access to indexed element of the state vector
      60              :   /// \param[in] idx  Index in the state vector
      61              :   /// \return const value_type&
      62            0 :   auto operator[](const sint32 idx) const -> value_type { return _vec.at_unsafe(idx); }
      63              : 
      64              :   /// \brief Read access to indexed element of the state covariance matrix
      65              :   /// \param[in,out] row  Row index in the state covariance matrix
      66              :   /// \param[in,out] col  Col index in the state covariance matrix
      67              :   /// \return value_type
      68            0 :   auto operator()(const sint32 row, const sint32 col) const -> value_type { return _cov.at_unsafe(row, col); }
      69              : 
      70              :   // clang-format off
      71              : TEST_REMOVE_PROTECTED:
      72              :   ; // workaround for correct indentation
      73              :   // clang-format on
      74              : 
      75              :   /// \brief Construct a new State Mem object
      76              :   /// \param[in] vec
      77              :   /// \param[in] cov
      78           20 :   explicit StateMem(const StateVec& vec, const StateCov& cov)
      79           20 :       : _vec{vec}
      80           20 :       , _cov{cov}
      81              :   {
      82           10 :   }
      83              : 
      84              :   /// \brief Write access to full state vector
      85              :   /// \return const StateVec&
      86          164 :   auto getVec() -> StateVec& { return _vec; }
      87              : 
      88              :   /// \brief Write access to state covariance matrix
      89              :   /// \return const StateCov&
      90            4 :   auto getCov() -> StateCov& { return _cov; }
      91              : 
      92              :   /// \brief Write access to indexed element of the state vector
      93              :   /// \param[in] idx  Index in the state vector
      94              :   /// \return value_type&
      95          200 :   auto operator[](const sint32 idx) -> value_type& { return _vec.at_unsafe(idx); }
      96              : 
      97              :   // clang-format off
      98              : TEST_REMOVE_PRIVATE:
      99              :   ; // workaround for correct indentation
     100              :   // clang-format on
     101              : 
     102              :   /// \brief State vector
     103              :   StateVec _vec{StateVec::Zeros()};
     104              :   /// \brief State covariance matrix
     105              :   StateCov _cov{StateCov::Identity()};
     106              : };
     107              : 
     108              : 
     109              : } // namespace motion
     110              : } // namespace tracking
     111              : #endif // D118B69B_C3E7_43F7_A2FC_F44B7ACF965F