INSTINCT Code Coverage Report

Directory: src/
File: util/Eigen.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 72 83 86.7%
Functions: 13 18 72.2%
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
20 #include <nlohmann/json.hpp>
21 using json = nlohmann::json; ///< json namespace
22
23 #include <fmt/ostream.h>
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 Matrix5d = Matrix<double, 5, 5>; ///< Double 5x5 Eigen::Matrix
34 using Matrix6d = Matrix<double, 6, 6>; ///< Double 6x6 Eigen::Matrix
35
36 using Array3ld = Array<long double, 3, 1>; ///< Long double 3x1 Eigen::Array
37
38 using Vector3ld = Matrix<long double, 3, 1>; ///< Long double 3x1 Eigen::Vector
39 using Vector4ld = Matrix<long double, 4, 1>; ///< Long double 3x1 Eigen::Vector
40
41 using Matrix3ld = Matrix<long double, 3, 3>; ///< Long double 3x3 Eigen::Matrix
42 using Matrix4ld = Matrix<long double, 4, 4>; ///< Long double 4x4 Eigen::Matrix
43
44 using Quaternionld = Quaternion<long double>; ///< Long double Eigen::Quaternion
45
46 using AngleAxisld = AngleAxis<long double>; ///< Long double Eigen::AngleAxis
47
48 /// @brief Converts the provided matrix into a json objetc
49 /// @tparam _Scalar Data Type of the matrix
50 /// @tparam _Rows Amount of rows of the matrix
51 /// @tparam _Cols Amount of cols of the matrix
52 /// @param[out] j Json object to fill with
53 /// @param[in] matrix Matrix to convert into json
54 template<typename _Scalar, int _Rows, int _Cols>
55 ✗ void to_json(json& j, const Matrix<_Scalar, _Rows, _Cols>& matrix)
56 {
57 ✗ for (int r = 0; r < matrix.rows(); r++)
58 {
59 ✗ for (int c = 0; c < matrix.cols(); c++)
60 {
61 ✗ if (std::isnan(matrix(r, c))) { j[std::to_string(r)][std::to_string(c)] = "NaN"; }
62 ✗ else { j[std::to_string(r)][std::to_string(c)] = matrix(r, c); }
63 }
64 }
65 ✗ }
66
67 /// @brief Converts the provided json object into a matrix
68 /// @tparam _Scalar Data Type of the matrix
69 /// @tparam _Rows Amount of rows of the matrix
70 /// @tparam _Cols Amount of cols of the matrix
71 /// @param[in] j Json object to read the coefficients from
72 /// @param[out] matrix Matrix object to fill
73 template<typename _Scalar, int _Rows, int _Cols>
74 1125 void from_json(const json& j, Matrix<_Scalar, _Rows, _Cols>& matrix)
75 {
76 if constexpr (_Rows == -1 || _Cols == -1)
77 {
78 20 int rows = -1;
79 20 int cols = -1;
80
3/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 90 times.
✓ Branch 6 taken 10 times.
 200 for (int r = 0; j.contains(std::to_string(r)); r++)
81 {
82 180 rows = std::max(r, rows);
83
4/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 90 times.
✓ Branch 11 taken 90 times.
 360 for (int c = 0; j.at(std::to_string(r)).contains(std::to_string(c)); c++)
84 {
85 180 cols = std::max(c, cols);
86 }
87 }
88
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 matrix = Eigen::Matrix<_Scalar, _Rows, _Cols>::Zero(rows + 1, cols + 1);
89 }
90
91
3/4
✓ Branch 2 taken 3944 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3021 times.
✓ Branch 6 taken 923 times.
 5004 for (int r = 0; j.contains(std::to_string(r)); r++) // NOLINT(readability-misleading-indentation)
92 {
93
4/6
✓ Branch 2 taken 6042 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6042 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 3021 times.
✓ Branch 11 taken 3021 times.
 7758 for (int c = 0; j.at(std::to_string(r)).contains(std::to_string(c)); c++)
94 {
95
3/6
✓ Branch 2 taken 3021 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3021 times.
✗ Branch 7 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 3021 times.
 3879 if (j.at(std::to_string(r)).at(std::to_string(c)).is_string())
96 {
97 ✗ auto str = j.at(std::to_string(r)).at(std::to_string(c)).get<std::string>();
98 ✗ if (str == "NaN") { matrix(r, c) = std::nan(""); }
99 ✗ else { LOG_WARN("Reading matrix value failed at position ({}, {}). Value is an unknown string '{}'.", r, c, str); }
100 ✗ }
101
3/6
✓ Branch 2 taken 3021 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3021 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 3021 times.
✗ Branch 12 not taken.
 3879 else if (j.at(std::to_string(r)).at(std::to_string(c)).is_number())
102 {
103
4/8
✓ Branch 2 taken 3021 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3021 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3021 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3021 times.
✗ Branch 13 not taken.
 3879 j.at(std::to_string(r)).at(std::to_string(c)).get_to(matrix(r, c));
104 }
105 else
106 {
107 ✗ 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());
108 }
109 }
110 }
111 1125 }
112
113 } // namespace Eigen
114
115 namespace NAV
116 {
117
118 /// @brief Removes rows from a matrix or vector
119 /// @param matrix Matrix to remove from
120 /// @param index Index to start removing
121 /// @param length Length to remove
122 template<typename Derived>
123 12 void removeRows(Eigen::DenseBase<Derived>& matrix, size_t index, size_t length)
124 {
125
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");
126
127 12 std::vector<int> indicesToKeep;
128
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 indicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - length);
129
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); }
130
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); }
131
132
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();
133 12 }
134
135 /// @brief Removes rows from a matrix or vector
136 /// @param matrix Matrix to remove from
137 /// @param rowIndices List with indices of rows to remove
138 template<typename Derived>
139 16 void removeRows(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& rowIndices)
140 {
141 16 std::vector<int> rowIndicesToKeep;
142
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 rowIndicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - rowIndices.size());
143
2/2
✓ Branch 1 taken 56 times.
✓ Branch 2 taken 8 times.
 128 for (int i = 0; i < matrix.rows(); i++)
144 {
145
3/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✓ Branch 6 taken 14 times.
 112 if (std::ranges::find(rowIndices, i) == rowIndices.end())
146 {
147
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 84 rowIndicesToKeep.push_back(i);
148 }
149 }
150
151
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 16 matrix = matrix(rowIndicesToKeep, Eigen::all).eval();
152 16 }
153
154 /// @brief Removes columns from a matrix or vector
155 /// @param matrix Matrix to remove from
156 /// @param index Index to start removing
157 /// @param length Length to remove
158 template<typename Derived>
159 12 void removeCols(Eigen::DenseBase<Derived>& matrix, size_t index, size_t length)
160 {
161
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");
162
163 12 std::vector<int> indicesToKeep;
164
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 indicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - length);
165
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); }
166
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); }
167
168
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();
169 12 }
170
171 /// @brief Removes cols from a matrix or vector
172 /// @param matrix Matrix to remove from
173 /// @param colIndices List with indices of cols to remove
174 template<typename Derived>
175 8 void removeCols(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& colIndices)
176 {
177 8 std::vector<int> colIndicesToKeep;
178
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 8 colIndicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - colIndices.size());
179
2/2
✓ Branch 1 taken 39 times.
✓ Branch 2 taken 6 times.
 62 for (int i = 0; i < matrix.cols(); i++)
180 {
181
3/4
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29 times.
✓ Branch 6 taken 10 times.
 54 if (std::ranges::find(colIndices, i) == colIndices.end())
182 {
183
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 41 colIndicesToKeep.push_back(i);
184 }
185 }
186
187
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.
 8 matrix = matrix(Eigen::all, colIndicesToKeep).eval();
188 8 }
189
190 /// @brief Removes rows and columns from a matrix or vector
191 /// @param matrix Matrix to remove from
192 /// @param row Row index to start removing
193 /// @param rows Amount of rows to remove
194 /// @param col Col index to start removing
195 /// @param cols Amount of cols to remove
196 template<typename Derived>
197 4 void removeRowsAndCols(Eigen::DenseBase<Derived>& matrix, size_t row, size_t rows, size_t col, size_t cols)
198 {
199
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");
200
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");
201
202 4 std::vector<int> rowsToKeep;
203
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 rowsToKeep.reserve(static_cast<size_t>(matrix.rows()) - rows);
204
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); }
205
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); }
206
207 4 std::vector<int> colsToKeep;
208
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 colsToKeep.reserve(static_cast<size_t>(matrix.cols()) - cols);
209
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); }
210
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); }
211
212
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();
213 4 }
214
215 /// @brief Removes rows and columns from a matrix or vector
216 /// @param matrix Matrix to remove from
217 /// @param rowIndices List with indices of rows to remove
218 /// @param colIndices List with indices of cols to remove
219 template<typename Derived>
220 13 void removeRowsAndCols(Eigen::DenseBase<Derived>& matrix, const std::vector<int>& rowIndices, const std::vector<int>& colIndices)
221 {
222 13 std::vector<int> rowIndicesToKeep;
223
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 rowIndicesToKeep.reserve(static_cast<size_t>(matrix.rows()) - rowIndices.size());
224
2/2
✓ Branch 1 taken 107 times.
✓ Branch 2 taken 13 times.
 120 for (int i = 0; i < matrix.rows(); i++)
225 {
226
3/4
✓ Branch 2 taken 107 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80 times.
✓ Branch 6 taken 27 times.
 107 if (std::ranges::find(rowIndices, i) == rowIndices.end())
227 {
228
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 rowIndicesToKeep.push_back(i);
229 }
230 }
231
232 13 std::vector<int> colIndicesToKeep;
233
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 colIndicesToKeep.reserve(static_cast<size_t>(matrix.cols()) - colIndices.size());
234
2/2
✓ Branch 1 taken 107 times.
✓ Branch 2 taken 13 times.
 120 for (int i = 0; i < matrix.cols(); i++)
235 {
236
3/4
✓ Branch 2 taken 107 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✓ Branch 6 taken 26 times.
 107 if (std::ranges::find(colIndices, i) == colIndices.end())
237 {
238
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 colIndicesToKeep.push_back(i);
239 }
240 }
241
242
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 matrix = matrix(rowIndicesToKeep, colIndicesToKeep).eval();
243 13 }
244
245 } // namespace NAV
246
247 #ifndef DOXYGEN_IGNORE
248
249 // clang-format off
250
251 template<typename T>
252 requires std::is_base_of_v<Eigen::DenseBase<T>, T>
253 struct fmt::formatter<T> : ostream_formatter
254 {};
255
256 // FIXME: This is not compiling with gcc 11.3 but with >12.1.
257 // template<typename T>
258 // requires std::is_base_of_v<Eigen::QuaternionBase<T>, T>
259 // struct fmt::formatter<T> : ostream_formatter
260 // {};
261 template<>
262 struct fmt::formatter<Eigen::Quaternionf> : ostream_formatter
263 {};
264 template<>
265 struct fmt::formatter<Eigen::Quaterniond> : ostream_formatter
266 {};
267 template<>
268 struct fmt::formatter<Eigen::Quaternionld> : ostream_formatter
269 {};
270
271 // clang-format on
272
273 #endif
274