INSTINCT Code Coverage Report

Directory: src/
File: util/Eigen.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 72 83 86.7%
Functions: 14 18 77.8%
Branches: 104 210 49.5%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file Eigen.hpp
10 /// @brief Vector space operations
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2021-01-05
13
14 #pragma once
15
16 #include <util/Logger.hpp>
17 #include <Eigen/Core>
18 #include <Eigen/Dense>
19 #include <fmt/ostream.h>
20 #include <fmt/ranges.h>
21
22 #include <nlohmann/json.hpp>
23 using json = nlohmann::json; ///< json namespace
24
25 #include "Assert.h"
26
27 namespace Eigen
28 {
29 using Array5d = Array<double, 5, 1>; ///< Double 5x1 Eigen::Array
30 using Array6d = Array<double, 6, 1>; ///< Double 6x1 Eigen::Array
31 using Vector5d = Matrix<double, 5, 1>; ///< Double 5x1 Eigen::Vector
32 using Vector6d = Matrix<double, 6, 1>; ///< Double 6x1 Eigen::Vector
33 using Vector7d = Matrix<double, 7, 1>; ///< Double 7x1 Eigen::Vector
34 using Vector8d = Matrix<double, 8, 1>; ///< Double 8x1 Eigen::Vector
35 using Vector9d = Matrix<double, 9, 1>; ///< Double 9x1 Eigen::Vector
36 using Matrix5d = Matrix<double, 5, 5>; ///< Double 5x5 Eigen::Matrix
37 using Matrix6d = Matrix<double, 6, 6>; ///< Double 6x6 Eigen::Matrix
38 using Matrix7d = Matrix<double, 7, 7>; ///< Double 7x7 Eigen::Matrix
39 using Matrix8d = Matrix<double, 8, 8>; ///< Double 8x8 Eigen::Matrix
40 using Matrix9d = Matrix<double, 9, 9>; ///< Double 9x9 Eigen::Matrix
41
42 using Array3ld = Array<long double, 3, 1>; ///< Long double 3x1 Eigen::Array
43
44 using Vector3ld = Matrix<long double, 3, 1>; ///< Long double 3x1 Eigen::Vector
45 using Vector4ld = Matrix<long double, 4, 1>; ///< Long double 3x1 Eigen::Vector
46
47 using Matrix3ld = Matrix<long double, 3, 3>; ///< Long double 3x3 Eigen::Matrix
48 using Matrix4ld = Matrix<long double, 4, 4>; ///< Long double 4x4 Eigen::Matrix
49
50 using Quaternionld = Quaternion<long double>; ///< Long double Eigen::Quaternion
51
52 using AngleAxisld = AngleAxis<long double>; ///< Long double Eigen::AngleAxis
53
54 /// @brief Converts the provided matrix into a json object
55 /// @tparam _Scalar Data Type of the matrix
56 /// @tparam _Rows Amount of rows of the matrix
57 /// @tparam _Cols Amount of cols of the matrix
58 /// @param[out] j Json object to fill with
59 /// @param[in] matrix Matrix to convert into json
60 template<typename _Scalar, int _Rows, int _Cols>
61 ✗ void to_json(json& j, const Matrix<_Scalar, _Rows, _Cols>& matrix)
62 {
63 ✗ for (int r = 0; r < matrix.rows(); r++)
64 {
65 ✗ for (int c = 0; c < matrix.cols(); c++)
66 {
67 ✗ if (std::isnan(matrix(r, c))) { j[std::to_string(r)][std::to_string(c)] = "NaN"; }
68 ✗ else { j[std::to_string(r)][std::to_string(c)] = matrix(r, c); }
69 }
70 }
71 ✗ }
72
73 /// @brief Converts the provided json object into a matrix
74 /// @tparam _Scalar Data Type of the matrix
75 /// @tparam _Rows Amount of rows of the matrix
76 /// @tparam _Cols Amount of cols of the matrix
77 /// @param[in] j Json object to read the coefficients from
78 /// @param[out] matrix Matrix object to fill
79 template<typename _Scalar, int _Rows, int _Cols>
80 627 void from_json(const json& j, Matrix<_Scalar, _Rows, _Cols>& matrix)
81 {
82 if constexpr (_Rows == -1 || _Cols == -1)
83 {
84 26 int rows = -1;
85 26 int cols = -1;
86
3/4
✓ Branch 2 taken 124 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✓ Branch 6 taken 16 times.
 224 for (int r = 0; j.contains(std::to_string(r)); r++)
87 {
88 198 rows = std::max(r, rows);
89
4/6
✓ Branch 2 taken 252 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 252 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 144 times.
✓ Branch 11 taken 108 times.
 432 for (int c = 0; j.at(std::to_string(r)).contains(std::to_string(c)); c++)
90 {
91 234 cols = std::max(c, cols);
92 }
93 }
94
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 26 matrix = Eigen::Matrix<_Scalar, _Rows, _Cols>::Zero(rows + 1, cols + 1);
95 }
96
97
3/4
✓ Branch 2 taken 2429 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1845 times.
✓ Branch 6 taken 584 times.
 2694 for (int r = 0; j.contains(std::to_string(r)); r++) // NOLINT(readability-misleading-indentation)
98 {
99
4/6
✓ Branch 2 taken 3726 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3726 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1881 times.
✓ Branch 11 taken 1845 times.
 4170 for (int c = 0; j.at(std::to_string(r)).contains(std::to_string(c)); c++)
100 {
101
3/6
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1881 times.
✗ Branch 7 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1881 times.
 2103 if (j.at(std::to_string(r)).at(std::to_string(c)).is_string())
102 {
103 ✗ auto str = j.at(std::to_string(r)).at(std::to_string(c)).get<std::string>();
104 ✗ if (str == "NaN") { matrix(r, c) = std::nan(""); }
105 ✗ else { LOG_WARN("Reading matrix value failed at position ({}, {}). Value is an unknown string '{}'.", r, c, str); }
106 ✗ }
107
3/6
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1881 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 1881 times.
✗ Branch 12 not taken.
 2103 else if (j.at(std::to_string(r)).at(std::to_string(c)).is_number())
108 {
109
4/8
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1881 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1881 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1881 times.
✗ Branch 13 not taken.
 2103 j.at(std::to_string(r)).at(std::to_string(c)).get_to(matrix(r, c));
110 }
111 else
112 {
113 ✗ LOG_WARN("Reading matrix value failed at position ({}, {}). Value has the type '{}' which cannot be converted into a floating point number.", r, c, j.at(std::to_string(r)).at(std::to_string(c)).type_name());
114 }
115 }
116 }
117 627 }
118
119 /// @brief Converts the provided quaternion into a json object
120 /// @tparam _Scalar Data Type of the quaternion
121 /// @param[out] j Json object to fill with
122 /// @param[in] quat Quaternion to convert into json
123 template<typename _Scalar>
124 void to_json(json& j, const Quaternion<_Scalar>& quat)
125 {
126 to_json(j, quat.coeffs());
127 }
128
129 /// @brief Converts the provided json object into a quaternion
130 /// @tparam _Scalar Data Type of the quaternion
131 /// @param[in] j Json object to read the coefficients from
132 /// @param[out] quat Quaternion object to fill
133 template<typename _Scalar>
134 void from_json(const json& j, Quaternion<_Scalar>& quat)
135 {
136 from_json(j, quat.coeffs());
137 }
138
139 } // namespace Eigen
140
141 namespace NAV
142 {
143
144 /// @brief Removes rows from a matrix or vector
145 /// @param matrix Matrix to remove from
146 /// @param index Index to start removing
147 /// @param length Length to remove
148 template<typename Derived>
149 12 void removeRows(Eigen::DenseBase<Derived>& matrix, size_t index, size_t length)
150 {
151
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.rows()) >= index + length, "Tried to remove rows which do not exist");
152
153 12 std::vector<int> indicesToKeep;
154
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 indicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - length);
155
3/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 6 times.
 30 for (int i = 0; i < static_cast<int>(index); i++) { indicesToKeep.push_back(i); }
156
3/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✓ Branch 5 taken 6 times.
 40 for (int i = static_cast<int>(index + length); i < matrix.rows(); i++) { indicesToKeep.push_back(i); }
157
158
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 12 matrix = matrix(indicesToKeep, Eigen::all).eval();
159 12 }
160
161 /// @brief Removes rows from a matrix or vector
162 /// @param matrix Matrix to remove from
163 /// @param rowIndices List with indices of rows to remove
164 template<typename Derived>
165 28 void removeRows(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& rowIndices)
166 {
167 28 std::vector<int> rowIndicesToKeep;
168
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 28 rowIndicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - rowIndices.size());
169
2/2
✓ Branch 1 taken 314 times.
✓ Branch 2 taken 14 times.
 656 for (int i = 0; i < matrix.rows(); i++)
170 {
171
3/4
✓ Branch 2 taken 314 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 294 times.
✓ Branch 6 taken 20 times.
 628 if (std::ranges::find(rowIndices, i) == rowIndices.end())
172 {
173
1/2
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
 588 rowIndicesToKeep.push_back(i);
174 }
175 }
176
177
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 matrix = matrix(rowIndicesToKeep, Eigen::all).eval();
178 28 }
179
180 /// @brief Removes columns from a matrix or vector
181 /// @param matrix Matrix to remove from
182 /// @param index Index to start removing
183 /// @param length Length to remove
184 template<typename Derived>
185 12 void removeCols(Eigen::DenseBase<Derived>& matrix, size_t index, size_t length)
186 {
187
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.cols()) >= index + length, "Tried to remove cols which do not exist");
188
189 12 std::vector<int> indicesToKeep;
190
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 indicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - length);
191
3/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 6 times.
 30 for (int i = 0; i < static_cast<int>(index); i++) { indicesToKeep.push_back(i); }
192
3/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✓ Branch 5 taken 6 times.
 40 for (int i = static_cast<int>(index + length); i < matrix.cols(); i++) { indicesToKeep.push_back(i); }
193
194
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 12 matrix = matrix(Eigen::all, indicesToKeep).eval();
195 12 }
196
197 /// @brief Removes cols from a matrix or vector
198 /// @param matrix Matrix to remove from
199 /// @param colIndices List with indices of cols to remove
200 template<typename Derived>
201 11 void removeCols(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& colIndices)
202 {
203 11 std::vector<int> colIndicesToKeep;
204
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 11 colIndicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - colIndices.size());
205
2/2
✓ Branch 1 taken 168 times.
✓ Branch 2 taken 9 times.
 194 for (int i = 0; i < matrix.cols(); i++)
206 {
207
3/4
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 155 times.
✓ Branch 6 taken 13 times.
 183 if (std::ranges::find(colIndices, i) == colIndices.end())
208 {
209
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 167 colIndicesToKeep.push_back(i);
210 }
211 }
212
213
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 11 matrix = matrix(Eigen::all, colIndicesToKeep).eval();
214 11 }
215
216 /// @brief Removes rows and columns from a matrix or vector
217 /// @param matrix Matrix to remove from
218 /// @param row Row index to start removing
219 /// @param rows Amount of rows to remove
220 /// @param col Col index to start removing
221 /// @param cols Amount of cols to remove
222 template<typename Derived>
223 4 void removeRowsAndCols(Eigen::DenseBase<Derived>& matrix, size_t row, size_t rows, size_t col, size_t cols)
224 {
225
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.rows()) >= row + rows, "Tried to remove rows which do not exist");
226
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 INS_ASSERT_USER_ERROR(static_cast<size_t>(matrix.cols()) >= col + cols, "Tried to remove cols which do not exist");
227
228 4 std::vector<int> rowsToKeep;
229
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 rowsToKeep.reserve(static_cast<size_t>(matrix.rows()) - rows);
230
3/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 4 times.
 5 for (int i = 0; i < static_cast<int>(row); i++) { rowsToKeep.push_back(i); }
231
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 4 times.
 9 for (int i = static_cast<int>(row + rows); i < matrix.rows(); i++) { rowsToKeep.push_back(i); }
232
233 4 std::vector<int> colsToKeep;
234
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 colsToKeep.reserve(static_cast<size_t>(matrix.cols()) - cols);
235
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 4 times.
 9 for (int i = 0; i < static_cast<int>(col); i++) { colsToKeep.push_back(i); }
236
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 4 times.
 10 for (int i = static_cast<int>(col + cols); i < matrix.cols(); i++) { colsToKeep.push_back(i); }
237
238
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 matrix = matrix(rowsToKeep, colsToKeep).eval();
239 4 }
240
241 /// @brief Removes rows and columns from a matrix or vector
242 /// @param matrix Matrix to remove from
243 /// @param rowIndices List with indices of rows to remove
244 /// @param colIndices List with indices of cols to remove
245 template<typename Derived>
246 31 void removeRowsAndCols(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& rowIndices, const std::vector<int>& colIndices)
247 {
248 31 std::vector<int> rowIndicesToKeep;
249
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 rowIndicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - rowIndices.size());
250
2/2
✓ Branch 1 taken 881 times.
✓ Branch 2 taken 31 times.
 912 for (int i = 0; i < matrix.rows(); i++)
251 {
252
3/4
✓ Branch 2 taken 881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 836 times.
✓ Branch 6 taken 45 times.
 881 if (std::ranges::find(rowIndices, i) == rowIndices.end())
253 {
254
1/2
✓ Branch 1 taken 836 times.
✗ Branch 2 not taken.
 836 rowIndicesToKeep.push_back(i);
255 }
256 }
257
258 31 std::vector<int> colIndicesToKeep;
259
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 colIndicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - colIndices.size());
260
2/2
✓ Branch 1 taken 881 times.
✓ Branch 2 taken 31 times.
 912 for (int i = 0; i < matrix.cols(); i++)
261 {
262
3/4
✓ Branch 2 taken 881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 837 times.
✓ Branch 6 taken 44 times.
 881 if (std::ranges::find(colIndices, i) == colIndices.end())
263 {
264
1/2
✓ Branch 1 taken 837 times.
✗ Branch 2 not taken.
 837 colIndicesToKeep.push_back(i);
265 }
266 }
267
268
3/6
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
 31 matrix = matrix(rowIndicesToKeep, colIndicesToKeep).eval();
269 31 }
270
271 } // namespace NAV
272
273 #ifndef DOXYGEN_IGNORE
274
275 // clang-format off
276
277 template<typename T>
278 requires(std::is_base_of_v<Eigen::DenseBase<T>, T>
279 && T::RowsAtCompileTime != 1
280 && T::ColsAtCompileTime != 1)
281 struct fmt::formatter<T> : ostream_formatter
282 {};
283
284 // template<typename T>
285 // requires std::is_base_of_v<Eigen::MatrixBase<T>, T>
286 // struct fmt::formatter<T> : ostream_formatter
287 // {};
288
289 // template<typename T>
290 // struct fmt::formatter<Eigen::MatrixBase<T>> : ostream_formatter
291 // {};
292
293 template<typename T>
294 requires std::is_base_of_v<Eigen::QuaternionBase<T>, T>
295 struct fmt::formatter<T> : ostream_formatter
296 {};
297
298 // template<typename T>
299 // struct fmt::formatter<Eigen::QuaternionBase<T>> : ostream_formatter
300 // {};
301
302 // clang-format on
303
304 #endif
305