Line data Source code
1 : #ifndef BE2AA4BF_8A4F_4A6D_8DFB_4E140C9F0A08
2 : #define BE2AA4BF_8A4F_4A6D_8DFB_4E140C9F0A08
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "math/linalg/errors.h" // IWYU pragma: keep
6 : #include "math/linalg/matrix.h" // IWYU pragma: keep
7 : #include "math/linalg/vector.h" // IWYU pragma: keep
8 : #include <initializer_list>
9 : #include <utility> // std::pair
10 :
11 : namespace tracking
12 : {
13 : namespace math
14 : {
15 :
16 : // forward declaration to prevent cyclic includes
17 : template <typename ValueType_, sint32 Size_, bool IsLower_, bool IsRowMajor_>
18 : class TriangularMatrix;
19 :
20 : // forward declaration to prevent cyclic includes
21 : template <typename ValueType_, sint32 Size_>
22 : class DiagonalMatrix;
23 :
24 : // TODO(matthias): add interface contract
25 :
26 : /// \brief A square matrix specialization of the Matrix class providing additional operations
27 : /// specific to square matrixes such as decompositions, inverse calculations, symmetry checks,
28 : /// and matrix property validation.
29 : ///
30 : /// This class inherits from Matrix<ValueType_, Size_, Size_, IsRowMajor_> and extends it with
31 : /// operations that are only meaningful for square matrixes. It supports various matrix
32 : /// decompositions (QR, LLT, LDLT, UDUT), matrix inversion, symmetry operations, and
33 : /// property checking functions for validation and debugging.
34 : ///
35 : /// \tparam ValueType_ The atomic data type of internal elements
36 : /// \tparam Size_ The dimension of the square matrix (compile-time constant)
37 : /// \tparam IsRowMajor_ Storage layout flag (true for row-major, false for column-major)
38 : ///
39 : /// \note All operations maintain the square property and may change storage layout for
40 : /// optimal performance (e.g., inverse operations may toggle IsRowMajor_)
41 : ///
42 : /// \see Matrix for the base matrix functionality
43 : /// \see TriangularMatrix for triangular matrix operations
44 : /// \see DiagonalMatrix for diagonal matrix operations
45 : template <typename ValueType_, sint32 Size_, bool IsRowMajor_ = true>
46 2529 : class SquareMatrix: public Matrix<ValueType_, Size_, Size_, IsRowMajor_>
47 : {
48 : public:
49 : using BaseMatrix = Matrix<ValueType_, Size_, Size_, IsRowMajor_>; ///< type of the parent class
50 :
51 : // unhide ctor of base class to allow implicit call in derived default ctors
52 324 : using BaseMatrix::BaseMatrix;
53 :
54 : //////////////////////////////////////////////////
55 : // additional constructors --->
56 : /// \brief Construct a new Square Matrix<ValueType_, Size_> object
57 : /// \param[in] other A base class object
58 0 : explicit SquareMatrix(const BaseMatrix& other)
59 0 : : BaseMatrix{other}
60 : {
61 : }
62 :
63 : /// \brief Move construct a new Square Matrix<ValueType_, Size_> object
64 : /// \param[in] other A base class object
65 1113 : explicit SquareMatrix(BaseMatrix&& other) noexcept
66 914 : : BaseMatrix{std::move(other)}
67 : {
68 : }
69 :
70 : /// \brief Construct a square matrix from a diagonal matrix.
71 : ///
72 : /// Creates a square matrix with the diagonal elements from the input diagonal matrix
73 : /// and zeros elsewhere.
74 : ///
75 : /// \param[in] other The diagonal matrix to convert from
76 : ///
77 : /// \note This is an implicit conversion constructor for convenience
78 : /// \note This constructor is defined in the implementation file to avoid circular dependencies
79 : SquareMatrix(const DiagonalMatrix<ValueType_, Size_>& other); // NOLINT(google-explicit-constructor)
80 :
81 :
82 : /// \brief Construct an identity matrix with ones on the diagonal and zeros elsewhere.
83 : ///
84 : /// \return SquareMatrix An identity matrix of the specified size and storage layout
85 : ///
86 : /// \note The identity matrix I satisfies I * A = A * I = A for any square matrix A
87 : [[nodiscard]] static auto Identity() -> SquareMatrix;
88 :
89 : /// \brief Creates a SquareMatrix from a nested initializer list
90 : ///
91 : /// This function constructs a SquareMatrix from a nested initializer list where each inner list
92 : /// represents a row of the matrix. The dimensions must be square and match the template parameter.
93 : ///
94 : /// \tparam ValueType_ The atomic data type of internal elements
95 : /// \tparam Size_ The dimension of the square matrix
96 : /// \tparam IsRowMajor_ The storage layout (true for row-major, false for column-major)
97 : /// \param[in] list Nested initializer list in logical row-major format
98 : /// \return SquareMatrix instance initialized with the provided values
99 : /// \throws std::runtime_error If the list dimensions don't match the square matrix size
100 : /// \see SquareFromDiagonal() for creating from diagonal matrixes
101 : /// \see MatrixFromList() for general matrix creation
102 : [[nodiscard]] static auto FromList(const std::initializer_list<std::initializer_list<ValueType_>>& list) -> SquareMatrix;
103 :
104 : /// \brief Set the matrix to the identity matrix in-place.
105 : ///
106 : /// Modifies the current matrix to have ones on the diagonal and zeros elsewhere.
107 : ///
108 : /// \note This operation modifies the matrix in-place and does not change its size or layout
109 : void setIdentity();
110 :
111 : /// \brief Fast transpose without changing the layout (zero-copy view)
112 : /// \return const transpose_type& const reference to same data as Self, but differently interpreted as transposed
113 : /// \note This creates a view with swapped dimensions and opposite layout. No data is copied.
114 : [[nodiscard]] auto transpose() const -> const SquareMatrix<ValueType_, Size_, !IsRowMajor_>&
115 : {
116 : return static_cast<const SquareMatrix<ValueType_, Size_, !IsRowMajor_>&>(BaseMatrix::transpose());
117 : };
118 :
119 : /// \brief Fast transpose without changing the layout (zero-copy view)
120 : /// \return transpose_type& reference to same data as Self, but differently interpreted as transposed
121 : /// \note This creates a view with swapped dimensions and opposite layout. No data is copied.
122 10 : [[nodiscard]] auto transpose() -> SquareMatrix<ValueType_, Size_, !IsRowMajor_>&
123 : {
124 : return static_cast<SquareMatrix<ValueType_, Size_, !IsRowMajor_>&>(BaseMatrix::transpose());
125 : };
126 :
127 : /// \brief Calculate the trace of the square matrix.
128 : ///
129 : /// Computes the sum of all diagonal elements of the matrix.
130 : /// The trace is defined as the sum of elements A_ii for i = 1 to n.
131 : ///
132 : /// \return ValueType_ The trace of the matrix(sum of diagonal elements)
133 : [[nodiscard]] auto trace() const -> ValueType_;
134 :
135 : /// \brief Calculate the determinant of the square matrix.
136 : ///
137 : /// Computes the determinant using LU decomposition with partial pivoting.
138 : /// The determinant is the product of the diagonal elements of the upper triangular
139 : /// matrix from the LU decomposition, multiplied by (-1)^k where k is the number
140 : /// of row permutations.
141 : ///
142 : /// \return ValueType_ The determinant of the matrix
143 : /// \note Time complexity: O(n^3) where n is the matrix dimension
144 : /// \note For singular matrixes, the determinant will be zero or very close to zero
145 : /// \note Uses partial pivoting for numerical stability
146 : [[nodiscard]] auto determinant() const -> ValueType_;
147 :
148 : /// \brief Performs Householder QR decomposition of a square matrix
149 : ///
150 : /// Decomposes a square matrix A into the product A = Q*R, where Q is an orthogonal matrix
151 : /// and R is an upper triangular matrix. This implementation uses Householder reflections
152 : /// for numerical stability.
153 : ///
154 : /// The Householder QR decomposition is based on successive Householder transformations that
155 : /// zero out subdiagonal elements column by column. Each Householder reflection is represented
156 : /// by a vector v and a scalar τ, where the reflection matrix is I - τ*v*v^T.
157 : ///
158 : /// \return std::pair<SquareMatrix, TriangularMatrix> A pair containing Q and R matrixes,
159 : /// where Q is orthogonal and R is upper triangular such that A = Q * R
160 : ///
161 : /// \note The decomposition satisfies A = Q * R, with Q being orthogonal (Q^T * Q = I)
162 : /// and R being upper triangular. This is useful for solving linear systems and
163 : /// computing matrix inverses.
164 : /// \note Time complexity: O(n^3) for an n x n matrix
165 : /// \note Space complexity: O(n^2) additional space for Q and R matrixes
166 : /// \note Numerically stable and suitable for general square matrixes
167 : ///
168 : /// \see qrSolve for solving linear systems using this decomposition
169 : /// \see inverse for matrix inversion using QR decomposition
170 : /// \see https://www.cs.cornell.edu/~bindel/class/cs6210-f09/lec18.pdf for algorithm reference
171 : [[nodiscard]] auto householderQR() const -> std::pair<SquareMatrix, TriangularMatrix<ValueType_, Size_, false, IsRowMajor_>>;
172 :
173 : /// \brief Solve the linear system A * x = b using QR decomposition.
174 : ///
175 : /// Uses the QR decomposition of this matrix to solve for x in A * x = b.
176 : /// This method is numerically stable and suitable for well-conditioned matrixes.
177 : ///
178 : /// \tparam IsRowMajor2_ The row-majority of the right-hand side matrix b
179 : /// \param[in] b The right-hand side matrix of the equation A * x = b
180 : /// \return Matrix The solution matrix x such that A * x ≈ b
181 : ///
182 : /// \note This method internally performs QR decomposition, which has O(n³) complexity.
183 : /// For multiple right-hand sides with the same A, consider using householderQR()
184 : /// directly and reusing the decomposition.
185 : ///
186 : /// \see householderQR for the underlying decomposition
187 : template <sint32 Cols_, bool IsRowMajor2_>
188 : [[nodiscard]] auto qrSolve(const Matrix<ValueType_, Size_, Cols_, IsRowMajor2_>& b) const
189 : -> Matrix<ValueType_, Size_, Cols_, !IsRowMajor_>;
190 :
191 40 : [[nodiscard]] auto qrSolve(const Vector<ValueType_, Size_>& b) const -> Vector<ValueType_, Size_>
192 : {
193 40 : return Vector<ValueType_, Size_>{qrSolve<1, true>(b)};
194 : }
195 :
196 : template <bool IsRowMajor2_>
197 82 : [[nodiscard]] auto qrSolve(const SquareMatrix<ValueType_, Size_, IsRowMajor2_>& b) const
198 : -> SquareMatrix<ValueType_, Size_, !IsRowMajor_>
199 : {
200 82 : return static_cast<SquareMatrix<ValueType_, Size_, !IsRowMajor_>>(qrSolve<Size_, IsRowMajor2_>(b));
201 : }
202 :
203 : /// \brief Performs Cholesky decomposition (LLT) of a symmetric positive definite matrix
204 : ///
205 : /// Decomposes a symmetric positive definite matrix A into the product A = L*L^T, where L
206 : /// is a lower triangular matrix with positive diagonal elements. This is also known as
207 : /// the Cholesky decomposition.
208 : ///
209 : /// The algorithm uses a recursive approach where each column of L is computed using
210 : /// previous columns. For a symmetric positive definite matrix, the Cholesky factor L
211 : /// satisfies L*L^T = A, with L being lower triangular.
212 : ///
213 : /// \return tl::expected<TriangularMatrix, Errors> The lower triangular matrix L on success,
214 : /// or Errors::matrix_not_symmetric if not symmetric, or
215 : /// Errors::matrix_not_positive_definite if not positive definite
216 : ///
217 : /// \note The matrix must be symmetric and positive definite
218 : /// \note Time complexity: O(n^3) for an n x n matrix
219 : /// \note Space complexity: O(n^2) for the result matrix
220 : /// \note Numerically stable for well-conditioned positive definite matrixes
221 : ///
222 : /// \warning The input matrix must be symmetric. Use symmetrize() if needed.
223 : /// \warning Fails if the matrix is not symmetric or not positive definite
224 : ///
225 : /// \see decomposeLDLT for a more numerically stable variant
226 : /// \see isSymmetric for symmetry checking
227 : [[nodiscard]] auto decomposeLLT() const -> tl::expected<TriangularMatrix<ValueType_, Size_, true, IsRowMajor_>, Errors>;
228 :
229 : /// \brief Performs LDL^T decomposition of a symmetric positive definite matrix
230 : ///
231 : /// Decomposes a symmetric positive definite matrix A into the product A = L*D*L^T, where
232 : /// L is a unit lower triangular matrix (diagonal elements are 1) and D is a diagonal
233 : /// matrix with positive diagonal elements.
234 : ///
235 : /// This decomposition is more numerically stable than LLT for certain matrixes and is
236 : /// particularly useful in Kalman filtering applications where the matrix structure
237 : /// needs to be maintained. The LDL^T form separates the triangular structure (L) from
238 : /// the scaling factors (D).
239 : ///
240 : /// \return tl::expected<std::pair<TriangularMatrix, DiagonalMatrix>, Errors>
241 : /// A pair containing L and D matrixes on success, or Errors::matrix_not_symmetric
242 : /// if not symmetric, or Errors::matrix_not_positive_definite if not positive definite
243 : ///
244 : /// \note The matrix must be symmetric and positive definite
245 : /// \note Time complexity: O(n^3) for an n x n matrix
246 : /// \note Space complexity: O(n^2) for L and O(n) for D
247 : /// \note More numerically stable than LLT for some applications
248 : /// \note L has unit diagonal (all diagonal elements are 1)
249 : ///
250 : /// \warning Fails if the matrix is not symmetric or not positive definite
251 : ///
252 : /// \see decomposeLLT for the standard Cholesky decomposition
253 : /// \see decomposeUDUT for the UDU^T variant
254 : [[nodiscard]] auto decomposeLDLT() const
255 : -> tl::expected<std::pair<TriangularMatrix<ValueType_, Size_, true, IsRowMajor_>, DiagonalMatrix<ValueType_, Size_>>,
256 : Errors>;
257 :
258 : /// \brief Performs UDU^T decomposition of a symmetric matrix
259 : ///
260 : /// Decomposes a symmetric matrix A into the product A = U*D*U^T, where U is a unit upper
261 : /// triangular matrix (diagonal elements are 1) and D is a diagonal matrix. This is also
262 : /// known as the UDU^T factorization.
263 : ///
264 : /// This decomposition is particularly important in Kalman filtering for maintaining the
265 : /// UDU^T form of covariance matrixes, which provides better numerical stability than
266 : /// standard covariance representations. The algorithm works backwards from the last
267 : /// column to the first, computing U and D simultaneously.
268 : ///
269 : /// The implementation is based on modified Cholesky decomposition and includes numerical
270 : /// safeguards to ensure positive definiteness even for near-singular matrixes.
271 : ///
272 : /// \return tl::expected<std::pair<TriangularMatrix, DiagonalMatrix>, Errors>
273 : /// A pair containing U and D matrixes on success, or Errors::matrix_not_symmetric
274 : /// if not symmetric
275 : ///
276 : /// \note The matrix must be symmetric
277 : /// \note Time complexity: O(n^3) for an n x n matrix
278 : /// \note Space complexity: O(n^2) for U and O(n) for D
279 : /// \note Numerically stable with safeguards for near-singular matrixes
280 : /// \note U has unit diagonal (all diagonal elements are 1)
281 : /// \note D diagonal elements are clamped to a minimum value for numerical stability
282 : ///
283 : /// \warning Fails if the matrix is not symmetric
284 : ///
285 : /// \see decomposeLDLT for the lower triangular variant
286 : /// \see CovarianceMatrixFactored for UDU usage in Kalman filters
287 : /// \see Grewal & Andrews, Kalman Filtering Theory and Practice Using MATLAB, 4th Edition, Wiley, 2014
288 : /// \see Gerald J. Bierman, "Factorization Methods for Discrete Sequential Estimation", 1977
289 : /// \see Catherine L. Thornton, "Triangular Covariance Factorizations for Kalman Filtering", 1976
290 : [[nodiscard]] auto decomposeUDUT() const
291 : -> tl::expected<std::pair<TriangularMatrix<ValueType_, Size_, false, IsRowMajor_>, DiagonalMatrix<ValueType_, Size_>>,
292 : Errors>;
293 :
294 : /// \brief Compute the matrix inverse using QR decomposition.
295 : ///
296 : /// Calculates the inverse of the matrix using QR decomposition for numerical stability.
297 : /// The result has toggled storage layout for optimal performance.
298 : ///
299 : /// \return SquareMatrix The inverse matrix such that A * A^(-1) = I
300 : ///
301 : /// \note Uses QR decomposition internally, which provides good numerical stability
302 : /// but has O(n³) complexity. The storage layout is toggled in the result.
303 : ///
304 : /// \warning Fails for singular or near-singular matrixes. Check condition number if needed.
305 : ///
306 : /// \see householderQR for the underlying decomposition
307 : [[nodiscard]] auto inverse() const -> SquareMatrix<ValueType_, Size_, !IsRowMajor_>;
308 :
309 : /// \brief Symmetrize the matrix by averaging with its transpose.
310 : ///
311 : /// Modifies the matrix to be symmetric by computing (A + A^T) / 2.
312 : /// This is useful for ensuring symmetry when small numerical errors
313 : /// have made a theoretically symmetric matrix asymmetric.
314 : ///
315 : /// \note This operation modifies the matrix in-place and ensures perfect symmetry
316 : /// at the cost of slightly changing the original values.
317 : void symmetrize();
318 :
319 : //////////////////////////////////////////////////
320 : // square matrix property checks --->
321 : /// \brief Check if the matrix is symmetric.
322 : ///
323 : /// Tests whether the matrix equals its transpose (A = A^T).
324 : ///
325 : /// \param tolerance Tolerance for numerical comparison (default: 1e-6)
326 : /// \return true if the matrix is symmetric, false otherwise
327 : ///
328 : /// \note Symmetry checking uses element-wise comparison and may have floating-point precision issues
329 : [[nodiscard]] auto isSymmetric(ValueType_ tolerance = 1e-6) const -> bool;
330 :
331 : /// \brief Check if the diagonal matrix is positive definite.
332 : ///
333 : /// A diagonal matrix is positive definite if all diagonal elements are positive.
334 : ///
335 : /// \return true if all diagonal elements are > 0, false otherwise
336 : ///
337 : /// \note For diagonal matrixes, positive definiteness is equivalent to all elements > 0
338 : [[nodiscard]] auto isPositiveDefinite() const -> bool;
339 :
340 : /// \brief Checks if the matrix is positive semi-definite.
341 : ///
342 : /// Tests whether all eigenvalues of the matrix are non-negative.
343 : /// A positive semi-definite matrix satisfies x^T * A * x >= 0 for all non-zero vectors x.
344 : ///
345 : /// This is a key property for covariance matrixes in statistics and machine learning.
346 : ///
347 : /// The check is performed using Cholesky decomposition; if it succeeds, the matrix
348 : /// is positive semi-definite.
349 : ///
350 : /// \return true if the matrix is positive semi-definite, false otherwise
351 : [[nodiscard]] auto isPositiveSemiDefinite() const -> bool;
352 :
353 : /// \brief Checks if the matrix is orthogonal within a specified tolerance
354 : ///
355 : /// Tests whether the matrix is orthogonal by checking if Q^T * Q = I within tolerance.
356 : /// An orthogonal matrix satisfies Q^T * Q = I, where I is the identity matrix.
357 : ///
358 : /// \param tolerance Tolerance for numerical comparison (default: 1e-6)
359 : /// \return true if matrix is orthogonal within tolerance, false otherwise
360 : ///
361 : /// \note This is useful for validating QR decomposition results
362 : /// \note Time complexity: O(n^3) due to matrix multiplication
363 : /// \see householderQR() for QR decomposition that produces orthogonal matrixes
364 : [[nodiscard]] auto isOrthogonal(ValueType_ tolerance = 1e-6) const -> bool;
365 :
366 : /// \brief Checks if the matrix is upper triangular within a specified tolerance
367 : ///
368 : /// Tests whether all elements below the main diagonal are zero within tolerance.
369 : /// An upper triangular matrix has all elements below the diagonal equal to zero.
370 : ///
371 : /// \param tolerance Tolerance for numerical comparison (default: 1e-6)
372 : /// \return true if matrix is upper triangular within tolerance, false otherwise
373 : ///
374 : /// \note This is useful for validating decomposition results (QR, UDUT)
375 : /// \note Time complexity: O(n^2)
376 : /// \see householderQR() for QR decomposition that produces upper triangular matrixes
377 : [[nodiscard]] auto isUpperTriangular(ValueType_ tolerance = 1e-6) const -> bool;
378 :
379 : /// \brief Checks if the matrix is lower triangular within a specified tolerance
380 : ///
381 : /// Tests whether all elements above the main diagonal are zero within tolerance.
382 : /// A lower triangular matrix has all elements above the diagonal equal to zero.
383 : ///
384 : /// \param tolerance Tolerance for numerical comparison (default: 1e-6)
385 : /// \return true if matrix is lower triangular within tolerance, false otherwise
386 : ///
387 : /// \note This is useful for validating decomposition results (LLT, LDLT)
388 : /// \note Time complexity: O(n^2)
389 : /// \see decomposeLLT() for LLT decomposition that produces lower triangular matrixes
390 : [[nodiscard]] auto isLowerTriangular(ValueType_ tolerance = 1e-6) const -> bool;
391 :
392 : /// \brief Checks if the matrix has a unit diagonal within a specified tolerance
393 : ///
394 : /// Tests whether all diagonal elements are equal to 1 within tolerance.
395 : /// A unit diagonal matrix has all diagonal elements equal to 1.
396 : ///
397 : /// \param tolerance Tolerance for numerical comparison (default: 1e-6)
398 : /// \return true if all diagonal elements are 1 within tolerance, false otherwise
399 : ///
400 : /// \note This is useful for validating decomposition results (LDLT, UDUT)
401 : /// \note Time complexity: O(n)
402 : /// \see decomposeLDLT() for LDLT decomposition that produces unit diagonal matrixes
403 : [[nodiscard]] auto hasUnitDiagonal(ValueType_ tolerance = 1e-6) const -> bool;
404 : // <---
405 :
406 : private:
407 : /// \brief Check if all diagonal elements are strictly positive.
408 : ///
409 : /// Tests whether all diagonal elements satisfy d_ii > 0.
410 : /// This is used internally for positive definiteness checks.
411 : ///
412 : /// \return true if all diagonal elements are positive, false otherwise
413 : ///
414 : /// \note This is a necessary but not sufficient condition for positive definiteness
415 : [[nodiscard]] auto hasStrictlyPositiveDiagonalElems() const -> bool;
416 : };
417 :
418 : } // namespace math
419 : } // namespace tracking
420 : #endif // BE2AA4BF_8A4F_4A6D_8DFB_4E140C9F0A08
|
