Line data    Source code

       1              : #ifndef A170ADCF_D2AE_4DF6_B6B6_D3DF0FA46A72
       2              : #define A170ADCF_D2AE_4DF6_B6B6_D3DF0FA46A72
       3              : 
       4              : #include "conversions.h"
       5              : #include "math/linalg/conversions/diagonal_conversions.hpp"   // IWYU pragma: keep
       6              : #include "math/linalg/conversions/square_conversions.hpp"     // IWYU pragma: keep
       7              : #include "math/linalg/conversions/triangular_conversions.hpp" // IWYU pragma: keep
       8              : #include "math/linalg/covariance_matrix_factored.hpp"         // IWYU pragma: keep
       9              : #include "math/linalg/covariance_matrix_full.hpp"             // IWYU pragma: keep
      10              : #include "math/linalg/square_matrix_decompositions.hpp"       // IWYU pragma: keep
      11              : #include <initializer_list>
      12              : 
      13              : namespace tracking
      14              : {
      15              : namespace math
      16              : {
      17              : namespace conversions
      18              : {
      19              : 
      20              : 
      21              : /// \brief Creates a CovarianceMatrixFactored from its full covariance matrix
      22              : ///
      23              : /// This function constructs a factored covariance matrix by decomposing the input
      24              : /// covariance matrix into UDU^T form. The input must be symmetric.
      25              : ///
      26              : /// \tparam ValueType_ The atomic data type of internal elements
      27              : /// \tparam Size_ The dimension of the covariance matrix
      28              : /// \param[in] other Full covariance matrix to decompose
      29              : /// \return tl::expected containing the CovarianceMatrixFactored instance on success
      30              : /// \see CovarianceMatrixFactoredFromList() (overloaded) for nested initializer list input
      31              : /// \see CovarianceMatrixFullFromList() for full covariance matrixes
      32              : template <typename ValueType_, sint32 Size_>
      33           44 : inline auto CovarianceMatrixFactoredFromCovarianceMatrixFull(
      34              :     const typename CovarianceMatrixFactored<ValueType_, Size_>::compose_type& other)
      35              :     -> tl::expected<CovarianceMatrixFactored<ValueType_, Size_>, Errors>
      36              : {
      37           44 :   const auto retVal = other.decomposeUDUT();
      38           44 :   if (retVal.has_value())
      39              :   {
      40           44 :     const auto [u, d] = retVal.value_or(std::make_pair(TriangularMatrix<ValueType_, Size_, false, true>::Identity(),
      41              :                                                        DiagonalMatrix<ValueType_, Size_>::Identity()));
      42           44 :     return CovarianceMatrixFactored{std::move(u), std::move(d)};
      43           44 :   }
      44            0 :   return tl::unexpected<Errors>{retVal.error()};
      45           44 : }
      46              : 
      47              : 
      48              : /// \brief Creates a CovarianceMatrixFactored from a nested initializer list
      49              : ///
      50              : /// This function constructs a factored covariance matrix by decomposing the input
      51              : /// covariance matrix into UDU^T form. The input must be symmetric.
      52              : ///
      53              : /// \tparam ValueType_ The atomic data type of internal elements
      54              : /// \tparam Size_ The dimension of the covariance matrix
      55              : /// \param[in] list Nested initializer list representing the full covariance matrix
      56              : /// \return CovarianceMatrixFactored instance with UDU^T decomposition of the input
      57              : /// \note The input matrix must be symmetric, otherwise assertion fails
      58              : /// \see CovarianceMatrixFactoredFromList() (overloaded) for separate U and D input
      59              : /// \see CovarianceMatrixFullFromList() for full covariance matrixes
      60              : template <typename ValueType_, sint32 Size_>
      61           21 : inline auto CovarianceMatrixFactoredFromList(const std::initializer_list<std::initializer_list<ValueType_>>& list)
      62              :     -> CovarianceMatrixFactored<ValueType_, Size_>
      63              : {
      64           21 :   const auto other = CovarianceMatrixFactored<ValueType_, Size_>::compose_type::FromList(list);
      65           21 :   const auto cov   = CovarianceMatrixFactoredFromCovarianceMatrixFull<ValueType_, Size_>(other);
      66           21 :   assert(cov.has_value() && "Input matrix cannot be factored into UDUt form");
      67           42 :   return cov.value();
      68           21 : }
      69              : 
      70              : } // namespace conversions
      71              : } // namespace math
      72              : } // namespace tracking
      73              : 
      74              : #endif // A170ADCF_D2AE_4DF6_B6B6_D3DF0FA46A72