Line data Source code
1 : #ifndef FDEAAACC_9EF1_4C87_94DC_2FA494822664
2 : #define FDEAAACC_9EF1_4C87_94DC_2FA494822664
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "math/linalg/contracts/matrix_intf.h"
6 : #include "math/linalg/errors.h"
7 : #include "math/linalg/matrix_types.h" // IWYU pragma: keep
8 : #include <algorithm>
9 : #include <array>
10 : #include <initializer_list>
11 : #include <tuple>
12 :
13 : namespace tracking
14 : {
15 : namespace math
16 : {
17 :
18 : // TODO(matthias): add support for external memory
19 : // TODO(matthias): add template params to support coord transformations (inFrame e.g. EGO_k_1, outFrame, power -> 2 for covs)
20 : /// Compliant to AUTOSAR C++14, A8-4-8: Output parameters shall not be used, making use of RVO and NRVO
21 :
22 : /// \brief A general-purpose matrix class implementing a Rows x Cols matrix with configurable storage layout.
23 : ///
24 : /// This class provides a comprehensive matrix implementation supporting both row-major and column-major
25 : /// storage layouts. It offers arithmetic operations, element access, block operations, and various
26 : /// matrix manipulations. All operations use compile-time dimension checking for type safety.
27 : ///
28 : /// Key features:
29 : /// - Configurable row-major or column-major storage layout
30 : /// - Compile-time dimension checking for type safety
31 : /// - Comprehensive arithmetic operations (+, -, *, /) with matrixes and scalars
32 : /// - Element access with bounds checking using tl::expected
33 : /// - Block operations for submatrix manipulation
34 : /// - Matrix multiplication with automatic result type deduction
35 : /// - Transpose operations with zero-copy views
36 : /// - Frobenius norm calculation for floating-point types
37 : ///
38 : /// \tparam ValueType_ The atomic data type of internal elements
39 : /// \tparam Rows_ The number of rows in the matrix (must be positive, > 0)
40 : /// \tparam Cols_ The number of columns in the matrix (must be positive, > 0)
41 : /// \tparam IsRowMajor_ Storage layout: true for row-major order, false for column-major order
42 : ///
43 : /// \note Row-major storage stores elements consecutively by row in memory (element [i][j] followed by [i][j+1]),
44 : /// while column-major stores by column (element [i][j] followed by [i+1][j]). The choice affects:
45 : /// - Memory access patterns and cache efficiency
46 : /// - Compatibility with external libraries (e.g., Eigen defaults to column-major)
47 : /// - Performance for row-wise vs column-wise operations
48 : ///
49 : /// \note Error handling uses the tl::expected<T, Errors> pattern throughout. Operations that can fail
50 : /// (e.g., division by zero, out-of-bounds access) return tl::expected containing either the result
51 : /// or an error descriptor from the Errors enum.
52 : ///
53 : /// \note All arithmetic operations support matrixes with different storage layouts but same dimensions.
54 : /// The result layout matches the left-hand side operand.
55 : ///
56 : /// \see Errors for possible error conditions and their meanings
57 : /// \see Matrix for usage examples below
58 : ///
59 : /// \warning Matrix dimensions are fixed at compile time. Dynamic sizing is not supported.
60 : ///
61 : /// Example usage:
62 : /// \code{.cpp}
63 : /// // Create a 3x2 row-major matrix of floats
64 : /// auto mat = Matrix<float32, 3, 2, true>::Zeros();
65 : ///
66 : /// // Element access with bounds checking
67 : /// auto result = mat(1, 0); // Returns tl::expected<float32, Errors>
68 : /// if (result) {
69 : /// float32 value = *result;
70 : /// } else {
71 : /// // Handle error: result.error() contains Errors enum value
72 : /// }
73 : ///
74 : /// // Arithmetic operations
75 : /// auto ones = Matrix<float32, 3, 2, true>::Ones();
76 : /// auto sum = mat + ones; // Element-wise addition
77 : /// auto scaled = mat * 2.0f; // Scalar multiplication
78 : ///
79 : /// // Matrix multiplication (3x2) * (2x4) = (3x4)
80 : /// auto other = Matrix<float32, 2, 4, true>::Ones();
81 : /// auto product = mat * other;
82 : ///
83 : /// // Transpose (creates view, zero-copy)
84 : /// auto transposed = mat.transpose(); // Type: Matrix<float32, 2, 3, false>
85 : /// \endcode
86 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_ = true>
87 : class Matrix: public contract::MatrixIntf<Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>>
88 : {
89 : public:
90 : //////////////////////////////////////////////////
91 : // type definitions --->
92 : /// \brief Type of the same matrix with opposite memory layout
93 : using opposite_mem_layout_type = Matrix<ValueType_, Rows_, Cols_, !IsRowMajor_>;
94 : /// \brief Type of the transposed matrix without changing the memory layout
95 : using transpose_type = Matrix<ValueType_, Cols_, Rows_, !IsRowMajor_>;
96 : /// \brief Type of the transposed matrix with fixed row major memory layout
97 : using transpose_type_row_major = Matrix<ValueType_, Cols_, Rows_, true>;
98 : using value_type = ValueType_; ///< element type
99 : /// \brief number of rows, columns, and memory layout
100 : static constexpr auto Rows = Rows_; ///< number of rows
101 : static constexpr auto Cols = Cols_; ///< number of cols
102 : static constexpr auto IsRowMajor = IsRowMajor_; ///< memory layout of the matrix
103 :
104 : // rule of 5 declarations
105 4464 : Matrix() = default;
106 1497 : Matrix(const Matrix&) = default;
107 972 : Matrix(Matrix&&) noexcept = default;
108 : auto operator=(const Matrix&) -> Matrix& = default;
109 360 : auto operator=(Matrix&&) noexcept -> Matrix& = default;
110 2321 : virtual ~Matrix() = default;
111 :
112 : //////////////////////////////////////////////////
113 : // additional constructors --->
114 : /// \brief Construct a Zero matrix
115 : /// \return Zero matrix
116 : [[nodiscard]] static auto Zeros() -> Matrix;
117 :
118 : /// \brief Construct a matrix filled with ones
119 : /// \return One matrix
120 : [[nodiscard]] static auto Ones() -> Matrix;
121 :
122 : /// \brief Creates a Matrix from a nested initializer list
123 : ///
124 : /// This function constructs a Matrix from a nested initializer list where each inner list
125 : /// represents a row of the matrix. The dimensions must match the template parameters exactly.
126 : ///
127 : /// \param[in] list Nested initializer list in logical row-major format
128 : /// \return Matrix instance initialized with the provided values
129 : /// \throws std::runtime_error If the list dimensions don't match the matrix dimensions
130 : [[nodiscard]] static auto FromList(const std::initializer_list<std::initializer_list<ValueType_>>& list) -> Matrix;
131 : // <---
132 :
133 : //////////////////////////////////////////////////
134 : // access operators --->
135 : /// \brief Element read-only access with bounds checking
136 : /// \param[in] row row of the element to read (0-based index)
137 : /// \param[in] col column of the element to read (0-based index)
138 : /// \return tl::expected<ValueType_, Errors> either the value at (row,col) or an Error descriptor
139 : /// \note Bounds checking is performed. Returns Errors::OutOfBounds if row < 0 || row >= Rows || col < 0 || col >= Cols
140 : [[nodiscard]] auto operator()(sint32 row, sint32 col) const -> tl::expected<ValueType_, Errors>;
141 :
142 : /// \brief Element modify access with bounds checking
143 : /// \param[in] row row of the element to modify (0-based index)
144 : /// \param[in] col column of the element to modify (0-based index)
145 : /// \return tl::expected<std::reference_wrapper<ValueType_>, Errors> either the reference at (row,col) or an Error descriptor
146 : /// \note Bounds checking is performed. Returns Errors::OutOfBounds if row < 0 || row >= Rows || col < 0 || col >= Cols
147 : [[nodiscard]] auto operator()(sint32 row, sint32 col) -> tl::expected<std::reference_wrapper<ValueType_>, Errors>;
148 : // <---
149 :
150 : //////////////////////////////////////////////////
151 : // comparison operators --->
152 : /// \brief Comparison to other matrix
153 : /// \tparam IsRowMajor2_ memory layout of other matrix
154 : /// \param[in] other matrix with any memory layout
155 : /// \return true if all elements are equal
156 : template <bool IsRowMajor2_>
157 : [[nodiscard]] auto operator==(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> bool;
158 :
159 : /// \brief Inequality comparison to other matrix
160 : /// \tparam IsRowMajor2_ memory layout of other matrix
161 : /// \param[in] other matrix with any memory layout
162 : /// \return true if any element differs
163 : template <bool IsRowMajor2_>
164 : [[nodiscard]] auto operator!=(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> bool;
165 : // <---
166 :
167 : //////////////////////////////////////////////////
168 : // arithmetic assignment operators --->
169 : /// \brief Calculates Self = Self + Other
170 : /// \tparam IsRowMajor2_ memory layout of other matrix
171 : /// \param[in] other matrix with any memory layout
172 : template <bool IsRowMajor2_>
173 : void operator+=(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other);
174 :
175 : /// \brief Calculates Self = Self - Other
176 : /// \tparam IsRowMajor2_ memory layout of other matrix
177 : /// \param[in] other matrix with any memory layout
178 : template <bool IsRowMajor2_>
179 : void operator-=(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other);
180 :
181 : /// \brief Calculates Self = Self * scalar
182 : /// \param[in] scalar scalar value
183 : void operator*=(ValueType_ scalar);
184 :
185 : /// \brief Calculates Self = Self / scalar for integral matrixes
186 : /// \param[in] scalar integral scalar value
187 : /// \return tl::expected<void, Errors> success or divide_by_zero error
188 : /// \note Division by zero (scalar == 0) returns Errors::DivideByZero
189 : template <typename T = ValueType_, typename std::enable_if_t<std::is_integral<T>::value, bool> = true>
190 : auto operator/=(T scalar) -> tl::expected<void, Errors>;
191 :
192 : /// \brief Calculates Self = Self / scalar for floating-point matrixes
193 : /// \param[in] scalar floating-point scalar value
194 : /// \return tl::expected<void, Errors> success or divide_by_zero error
195 : /// \note Division by zero (scalar == 0.0) returns Errors::DivideByZero
196 : template <typename T = ValueType_, typename std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
197 : auto operator/=(T scalar) -> tl::expected<void, Errors>;
198 : // <---
199 :
200 : //////////////////////////////////////////////////
201 : // arithmentic operators --->
202 : /// \brief Calculates Self + Other
203 : /// \tparam IsRowMajor2_ memory layout of other matrix
204 : /// \param[in] other matrix with any memory layout
205 : /// \return Matrix result of Self + Other
206 : template <bool IsRowMajor2_>
207 : [[nodiscard]] auto operator+(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> Matrix;
208 :
209 : /// \brief Calculates Self - Other
210 : /// \tparam IsRowMajor2_ memory layout of other matrix
211 : /// \param[in] other matrix with any memory layout
212 : /// \return Matrix result of Self - Other
213 : template <bool IsRowMajor2_>
214 : [[nodiscard]] auto operator-(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> Matrix;
215 :
216 : /// \brief Calculates Self + scalar (adds scalar to each element)
217 : /// \param[in] scalar a scalar value
218 : /// \return Matrix result of Self + scalar
219 : [[nodiscard]] auto operator+(ValueType_ scalar) const -> Matrix;
220 :
221 : /// \brief Calculates Self - scalar (subtracts scalar from each element)
222 : /// \param[in] scalar a scalar value
223 : /// \return Matrix result of Self - scalar
224 : [[nodiscard]] auto operator-(ValueType_ scalar) const -> Matrix;
225 :
226 : /// \brief Calculates Self * scalar
227 : /// \param[in] scalar a scalar value
228 : /// \return Matrix result of Self * scalar
229 : [[nodiscard]] auto operator*(ValueType_ scalar) const -> Matrix;
230 :
231 : /// \brief Calculates Self / scalar for integral matrixes
232 : /// \param[in] scalar a scalar value
233 : /// \return tl::expected<Matrix, Errors> either the result Self / scalar or an Error descriptor
234 : /// \note Division by zero (scalar == 0) returns Errors::DivideByZero
235 : template <typename T = ValueType_, typename std::enable_if_t<std::is_integral<T>::value, bool> = true>
236 : [[nodiscard]] auto operator/(T scalar) const -> tl::expected<Matrix, Errors>;
237 :
238 : /// \brief Calculates Self / scalar for floating-point matrixes
239 : /// \param[in] scalar a scalar value
240 : /// \return tl::expected<Matrix, Errors> either the result Self / scalar or an Error descriptor
241 : /// \note Division by zero (scalar == 0.0) returns Errors::DivideByZero
242 : template <typename T = ValueType_, typename std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
243 : [[nodiscard]] auto operator/(T scalar) const -> tl::expected<Matrix, Errors>;
244 :
245 : /// \brief Calculates matrix multiplication Self * Other
246 : /// \tparam Cols2_ Number of columns in the other matrix
247 : /// \tparam IsRowMajor2_ Storage layout of the other matrix
248 : /// \param[in] other Right-hand side matrix for multiplication
249 : /// \return Matrix<ValueType_, Rows_, Cols2_, IsRowMajor_> Result matrix with dimensions (Rows x Cols2)
250 : /// \note Matrix multiplication requires that Self.Cols == Other.Rows. The result layout matches the left operand.
251 : template <sint32 Cols2_, bool IsRowMajor2_>
252 : [[nodiscard]] auto operator*(const Matrix<ValueType_, Cols_, Cols2_, IsRowMajor2_>& other) const
253 : -> Matrix<ValueType_, Rows_, Cols2_, IsRowMajor_>;
254 : // <---
255 :
256 : //////////////////////////////////////////////////
257 : // other operations --->
258 : /// \brief Sets all elements to zero
259 : void setZeros();
260 :
261 : /// \brief Sets all elements to one
262 : void setOnes();
263 :
264 : /// \brief Checks if the matrix is a zero matrix
265 : /// \return true if all elements are zero
266 : [[nodiscard]] auto isZeros() const -> bool;
267 :
268 : /// \brief Get min value of the matrix
269 : /// \return min value
270 : [[nodiscard]] auto min() const -> ValueType_ { return *std::min_element(data().begin(), data().end()); }
271 :
272 : /// \brief Get max value of the matrix
273 : /// \return max value
274 : [[nodiscard]] auto max() const -> ValueType_ { return *std::max_element(data().begin(), data().end()); }
275 :
276 : /// \brief Get min and max value of the matrix
277 : /// \return tuple of min, max value
278 : [[nodiscard]] auto minmax() const -> std::tuple<ValueType_, ValueType_>;
279 :
280 : /// \brief Calculate Frobenius norm (L2 norm): sqrt(sum of squared elements)
281 : /// \note Only available for floating-point types. For integral types, this method is not instantiated.
282 : template <typename T = ValueType_, typename std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
283 : [[nodiscard]] auto frobenius_norm() const -> ValueType_;
284 :
285 : /// \brief Fast transpose without changing the layout (zero-copy view)
286 : /// \return const transpose_type& const reference to same data as Self, but differently interpreted as transposed
287 : /// \note This creates a view with swapped dimensions and opposite layout. No data is copied.
288 : [[nodiscard]] auto transpose() const -> const transpose_type&;
289 :
290 : /// \brief Fast transpose without changing the layout (zero-copy view)
291 : /// \return transpose_type& reference to same data as Self, but differently interpreted as transposed
292 : /// \note This creates a view with swapped dimensions and opposite layout. No data is copied.
293 : [[nodiscard]] auto transpose() -> transpose_type&;
294 :
295 : /// \brief Fast transpose without changing the layout (rvalue version)
296 : /// \return transpose_type rvalue reference to same data as Self, but differently interpreted as transposed
297 : /// \note This creates a view with swapped dimensions and opposite layout. No data is copied. Use for temporary objects.
298 : [[nodiscard]] auto transpose_rvalue() && -> transpose_type;
299 :
300 : /// \brief Set a block matrix at given position (compile-time parameters)
301 : /// \tparam SrcRowSize_ Rows of the source block matrix
302 : /// \tparam SrcColSize_ Cols of the source block matrix
303 : /// \tparam SrcRowCount_ Number of rows to copy from source (must be > 0)
304 : /// \tparam SrcColCount_ Number of cols to copy from source (must be > 0)
305 : /// \tparam SrcRowBeg_ Begin row index in source (0-based, must be >= 0)
306 : /// \tparam SrcColBeg_ Begin col index in source (0-based, must be >= 0)
307 : /// \tparam SrcIsRowMajor_ Memory layout of source matrix
308 : /// \tparam DstRowBeg_ Begin row index in destination (0-based, must be >= 0)
309 : /// \tparam DstColBeg_ Begin col index in destination (0-based, must be >= 0)
310 : /// \param[in] block Source block matrix to copy from
311 : /// \note All indices and counts are compile-time constants. The copied region must fit within both matrixes. No runtime bounds
312 : /// checking.
313 : template <sint32 SrcRowSize_,
314 : sint32 SrcColSize_,
315 : sint32 SrcRowCount_,
316 : sint32 SrcColCount_,
317 : sint32 SrcRowBeg_,
318 : sint32 SrcColBeg_,
319 : bool SrcIsRowMajor_,
320 : sint32 DstRowBeg_,
321 : sint32 DstColBeg_>
322 : void setBlock(const Matrix<ValueType_, SrcRowSize_, SrcColSize_, SrcIsRowMajor_>& block);
323 :
324 : /// \brief Set a block matrix at given position
325 : /// \tparam SrcRowSize_ Rows of the source block matrix
326 : /// \tparam SrcColSize_ Cols of the source block matrix
327 : /// \tparam SrcIsRowMajor_ Memory layout of source matrix
328 : /// \param srcRowCount Number of rows to copy from source (must be > 0)
329 : /// \param srcColCount Number of cols to copy from source (must be > 0)
330 : /// \param srcRowBeg Begin row index in source (0-based, must be >= 0)
331 : /// \param srcColBeg Begin col index in source (0-based, must be >= 0)
332 : /// \param dstRowBeg Begin row index in destination (0-based, must be >= 0)
333 : /// \param dstColBeg Begin col index in destination (0-based, must be >= 0)
334 : /// \param[in] block Source block matrix to copy from
335 : /// \note The copied region must fit within both source and destination bounds. No bounds checking is performed.
336 : template <sint32 SrcRowSize_, sint32 SrcColSize_, bool SrcIsRowMajor_>
337 : void setBlock(const sint32 srcRowCount,
338 : const sint32 srcColCount,
339 : const sint32 srcRowBeg,
340 : const sint32 srcColBeg,
341 : const sint32 dstRowBeg,
342 : const sint32 dstColBeg,
343 : const Matrix<ValueType_, SrcRowSize_, SrcColSize_, SrcIsRowMajor_>& block);
344 : // <---
345 :
346 : //////////////////////////////////////////////////
347 : // unsafe access operators --->
348 : /// \brief Unsafe element read-only access (no bounds checking)
349 : /// \param[in] row row of the element to read (0-based index, must be valid)
350 : /// \param[in] col column of the element to read (0-based index, must be valid)
351 : /// \return ValueType_ the value at (row,col)
352 : /// \warning No bounds checking is performed. Undefined behavior if indices are out of bounds.
353 : [[nodiscard]] auto at_unsafe(sint32 row, sint32 col) const -> ValueType_;
354 :
355 : /// \brief Unsafe element modify access (no bounds checking)
356 : /// \param[in] row row of the element to modify (0-based index, must be valid)
357 : /// \param[in] col column of the element to modify (0-based index, must be valid)
358 : /// \return ValueType_& reference to the value at (row,col)
359 : /// \warning No bounds checking is performed. Undefined behavior if indices are out of bounds.
360 : [[nodiscard]] auto at_unsafe(sint32 row, sint32 col) -> ValueType_&;
361 : // <---
362 :
363 : // clang-format off
364 : TEST_REMOVE_PROTECTED:
365 : ; // workaround for correct indentation
366 : // clang-format on
367 : /// \brief number of rows and columns in memory layout
368 : static constexpr auto RowsInMem = IsRowMajor_ ? Rows_ : Cols_;
369 : static constexpr auto ColsInMem = IsRowMajor_ ? Cols_ : Rows_;
370 :
371 : using Storage = std::array<ValueType_, static_cast<sint32>(Rows* Cols)>; ///< type of the internal storage
372 :
373 : //////////////////////////////////////////////////
374 : // access operators --->
375 : /// \brief Read-only access to the internal data
376 : /// \return const Storage&
377 10861 : auto data() const -> const Storage& { return _data; }
378 :
379 : /// \brief Modify access to the internal data
380 : /// \return Storage&
381 6287 : auto data() -> Storage& { return _data; }
382 : // <---
383 :
384 : // clang-format off
385 : TEST_REMOVE_PRIVATE:
386 : ; // workaround for correct indentation
387 : // clang-format on
388 : template <typename T = ValueType_, typename std::enable_if_t<std::is_integral<T>::value, bool> = true>
389 : void inplace_div_by_int_unsafe(T scalar);
390 :
391 : template <typename T = ValueType_, typename std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
392 : void inplace_mul_by_inverse_factor_unsafe(T scalar);
393 :
394 : Storage _data{}; ///< internal data to store the matrix
395 : };
396 :
397 : //////////////////////////////////////////////////
398 : // non member operations --->
399 :
400 : /// \brief Calculates Scalar + Matrix (element-wise addition)
401 : /// \tparam ValueType_ The atomic data type of internal elements
402 : /// \tparam Rows_ Number of rows in the matrix
403 : /// \tparam Cols_ Number of columns in the matrix
404 : /// \tparam IsRowMajor_ Storage layout of the matrix
405 : /// \param[in] scalar Scalar value to add to each element
406 : /// \param[in] mat Matrix operand
407 : /// \return Matrix<ValueType_, Rows_, Cols_, IsRowMajor_> Result of scalar + matrix
408 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
409 3 : [[nodiscard]] static auto operator+(ValueType_ scalar, const Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>& mat)
410 : -> Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>
411 : {
412 3 : return mat + scalar;
413 : }
414 :
415 : /// \brief Calculates Scalar * Matrix (element-wise multiplication)
416 : /// \tparam ValueType_ The atomic data type of internal elements
417 : /// \tparam Rows_ Number of rows in the matrix
418 : /// \tparam Cols_ Number of columns in the matrix
419 : /// \tparam IsRowMajor_ Storage layout of the matrix
420 : /// \param[in] scalar Scalar value to multiply with each element
421 : /// \param[in] mat Matrix operand
422 : /// \return Matrix<ValueType_, Rows_, Cols_, IsRowMajor_> Result of scalar * matrix
423 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
424 6 : [[nodiscard]] static auto operator*(ValueType_ scalar, const Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>& mat)
425 : -> Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>
426 : {
427 6 : return mat * scalar;
428 : }
429 : // <---
430 :
431 : } // namespace math
432 : } // namespace tracking
433 :
434 : #endif // FDEAAACC_9EF1_4C87_94DC_2FA494822664
|
