INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/PolynomialRegressor.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 73 83 88.0%
Functions: 12 12 100.0%
Branches: 50 95 52.6%
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 PolynomialRegressor.hpp
10 /// @brief Polynomial curve fitting
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-10-23
13
14 #pragma once
15
16 #include <cstddef>
17 #include <utility>
18
19 #include <nlohmann/json.hpp>
20 using json = nlohmann::json; ///< json namespace
21
22 #include "util/Assert.h"
23 #include "util/Container/ScrollingBuffer.hpp"
24
25 #include "Polynomial.hpp"
26 #include "internal/PolynomialRegressor/IncrementalLeastSquares.hpp"
27 #include "internal/PolynomialRegressor/LeastSquares.hpp"
28 #include "internal/PolynomialRegressor/HouseholderQr.hpp"
29 #include "internal/PolynomialRegressor/BDCSVD.hpp"
30 #include "internal/PolynomialRegressor/COD.hpp"
31
32 namespace NAV
33 {
34
35 /// @brief Polynomial Curve Fitting
36 /// @tparam Scalar Data type to store
37 template<typename Scalar = double>
38 class PolynomialRegressor
39 {
40 public:
41 /// Possible Fit strategies
42 enum class Strategy : uint8_t
43 {
44 IncrementalLeastSquares, ///< Incremental Least Squares (only polynomials of order <= 2)
45 LeastSquares, ///< Least Squares (bas if even mildly ill-conditioned)
46 HouseholderQR, ///< Householder QR decomposition
47 BDCSVD, ///< Bidiagonal Divide and Conquer SVD
48 COD, ///< Complete Orthogonal Decomposition
49 COUNT, ///< Amount of items in the enum
50 };
51
52 /// @brief Constructor
53 /// @param[in] polynomialDegree Degree of the polynomial to fit
54 /// @param[in] windowSize Amount of points to use for the fit (sliding window)
55 /// @param[in] strategy Strategy to use
56 2418 PolynomialRegressor(size_t polynomialDegree, size_t windowSize, Strategy strategy = Strategy::HouseholderQR)
57
1/2
✓ Branch 2 taken 2418 times.
✗ Branch 3 not taken.
 2418 : _strategy(strategy), _polyDegree(polynomialDegree), _windowSize(windowSize), _incrementalLSQ(polynomialDegree)
58 {
59
1/2
✓ Branch 1 taken 2418 times.
✗ Branch 2 not taken.
 2418 setWindowSize(windowSize);
60
1/2
✓ Branch 1 taken 2418 times.
✗ Branch 2 not taken.
 2418 setPolynomialDegree(polynomialDegree);
61 2418 }
62
63 /// @brief Sets the amount of points used for the fit (sliding window)
64 /// @param[in] windowSize Amount of points to use for the fit
65 2418 void setWindowSize(size_t windowSize)
66 {
67
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2418 times.
 2418 INS_ASSERT_USER_ERROR(windowSize > _polyDegree, "The window size needs to be greater than the polynomial degree.");
68
69
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2418 times.
 2418 while (windowSize < _windowSize)
70 {
71 ✗ pop_front();
72 ✗ _windowSize--;
73 }
74
75 2418 _windowSize = windowSize;
76 2418 _data.resize(windowSize);
77 2418 }
78
79 /// @brief Set the Polynomial Degree and resets the data
80 /// @param[in] polynomialDegree Degree of the polynomial to fit
81 2418 void setPolynomialDegree(size_t polynomialDegree)
82 {
83
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2418 times.
 2418 INS_ASSERT_USER_ERROR(polynomialDegree < _windowSize, "The polynomial degree needs to be smaller than the window size.");
84 2418 _polyDegree = polynomialDegree;
85
86 2418 _incrementalLSQ.setPolynomialDegree(polynomialDegree);
87
88 2418 reset();
89 2418 }
90
91 /// @brief Set the strategy for the fit and resets the data
92 /// @param strategy Strategy to use for fitting data
93 2160 void setStrategy(Strategy strategy)
94 {
95
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2160 times.
 2160 INS_ASSERT_USER_ERROR(strategy != Strategy::COUNT, "You cannot call this function with COUNT strategy");
96
97 2160 _strategy = strategy;
98 2160 reset();
99 2160 }
100
101 /// @brief Add a data point to the polynomial
102 /// @param[in] dataPoint Data point
103 5845709 void push_back(const std::pair<Scalar, Scalar>& dataPoint)
104 {
105 5845709 push_back(dataPoint.first, dataPoint.second);
106 5845709 }
107
108 /// @brief Add a data point to the polynomial
109 /// @param[in] x X Value
110 /// @param[in] y Y Value
111 5904221 void push_back(const Scalar& x, const Scalar& y)
112 {
113
4/6
✓ Branch 1 taken 5902040 times.
✓ Branch 2 taken 2181 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 5902040 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 5904221 times.
 5904221 if (!_data.empty() && _data.back().first == x)
114 {
115 ✗ if (_strategy == Strategy::IncrementalLeastSquares)
116 {
117 ✗ _incrementalLSQ.removeDataPoint(_data.back().first, _data.back().second);
118 ✗ _incrementalLSQ.addDataPoint(x, y);
119 }
120 ✗ _data.back().second = y;
121
122 ✗ return;
123 }
124
125
2/2
✓ Branch 1 taken 293500 times.
✓ Branch 2 taken 5610721 times.
 5904221 if (_data.full()) { pop_front(); }
126
127
2/2
✓ Branch 0 taken 1122705 times.
✓ Branch 1 taken 4781516 times.
 5904221 if (_strategy == Strategy::IncrementalLeastSquares)
128 {
129 1122705 _incrementalLSQ.addDataPoint(x, y);
130 }
131
132
1/2
✓ Branch 2 taken 5904221 times.
✗ Branch 3 not taken.
 5904221 _data.push_back(std::make_pair(x, y));
133 }
134
135 /// @brief Reset the polynomial coefficients and saved data
136 4620 void reset()
137 {
138 4620 _incrementalLSQ.reset();
139 4620 _data.clear();
140 4620 }
141
142 /// @brief Calculates the polynomial
143 351183 [[nodiscard]] Polynomial<Scalar> calcPolynomial() const
144 {
145 690264 auto prepareDataVectors = [&]() {
146 339081 auto n = static_cast<int>(_data.size());
147
1/2
✓ Branch 1 taken 339081 times.
✗ Branch 2 not taken.
 339081 Eigen::VectorX<Scalar> x = Eigen::VectorX<Scalar>(n);
148
1/2
✓ Branch 1 taken 339081 times.
✗ Branch 2 not taken.
 339081 Eigen::VectorX<Scalar> y = Eigen::VectorX<Scalar>(n);
149
150
2/2
✓ Branch 1 taken 25319949 times.
✓ Branch 2 taken 339081 times.
 25659030 for (size_t i = 0; i < _data.size(); i++)
151 {
152
2/4
✓ Branch 1 taken 25319949 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25319949 times.
✗ Branch 5 not taken.
 25319949 x(static_cast<int>(i)) = _data.at(i).first;
153
2/4
✓ Branch 1 taken 25319949 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25319949 times.
✗ Branch 5 not taken.
 25319949 y(static_cast<int>(i)) = _data.at(i).second;
154 }
155
156
1/2
✓ Branch 1 taken 339081 times.
✗ Branch 2 not taken.
 678162 return std::make_pair(x, y);
157 339081 };
158
159
5/7
✓ Branch 0 taken 12102 times.
✓ Branch 1 taken 12102 times.
✓ Branch 2 taken 302775 times.
✓ Branch 3 taken 12102 times.
✓ Branch 4 taken 12102 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 351183 switch (_strategy)
160 {
161 12102 case Strategy::IncrementalLeastSquares:
162
2/4
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12102 times.
✗ Branch 5 not taken.
 12102 return { _incrementalLSQ.calcCoefficients() };
163 12102 case Strategy::LeastSquares:
164 {
165
1/2
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
 12102 auto [x, y] = prepareDataVectors();
166
2/4
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12102 times.
✗ Branch 5 not taken.
 12102 return { LeastSquares<Scalar>::calcCoefficients(x, y, _polyDegree) };
167 12102 }
168 302775 case Strategy::HouseholderQR:
169 {
170
1/2
✓ Branch 1 taken 302775 times.
✗ Branch 2 not taken.
 302775 auto [x, y] = prepareDataVectors();
171
2/4
✓ Branch 1 taken 302775 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 302775 times.
✗ Branch 5 not taken.
 302775 return { HouseholderQr<Scalar>::calcCoefficients(x, y, _polyDegree) };
172 302775 }
173 12102 case Strategy::BDCSVD:
174 {
175
1/2
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
 12102 auto [x, y] = prepareDataVectors();
176
2/4
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12102 times.
✗ Branch 5 not taken.
 12102 return { BDCSVD<Scalar>::calcCoefficients(x, y, _polyDegree) };
177 12102 }
178 12102 case Strategy::COD:
179 {
180
1/2
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
 12102 auto [x, y] = prepareDataVectors();
181
2/4
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12102 times.
✗ Branch 5 not taken.
 12102 return { COD<Scalar>::calcCoefficients(x, y, _polyDegree) };
182 12102 }
183 ✗ case Strategy::COUNT:
184 ✗ break;
185 }
186 ✗ return { Eigen::VectorX<Scalar>() };
187 }
188
189 /// @brief Checks if the amount of data points equals the window size
190 6 [[nodiscard]] bool windowSizeReached() const
191 {
192 6 return _data.size() == _windowSize;
193 }
194
195 /// @brief Checks if the container has no elements
196 581328 [[nodiscard]] bool empty() const { return _data.empty(); }
197
198 /// @brief Gets the underlying buffer
199 [[nodiscard]] const ScrollingBuffer<std::pair<Scalar, Scalar>>& data() const { return _data; }
200
201 private:
202 /// Strategy to use to fit the polynomial
203 Strategy _strategy = Strategy::IncrementalLeastSquares;
204 /// Polynomial degree to fit
205 size_t _polyDegree = 2;
206 /// Amount of points to store
207 size_t _windowSize = 10;
208 /// Values added to the fit
209 ScrollingBuffer<std::pair<Scalar, Scalar>> _data;
210 /// Incremental LSQ Regressor
211 IncrementalLeastSquares<Scalar> _incrementalLSQ;
212
213 /// @brief Removes the first data point from the polynomial fit (sliding window)
214 293500 void pop_front()
215 {
216
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 293500 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 293500 if (_data.empty()) { return; }
217
218
1/2
✓ Branch 1 taken 293500 times.
✗ Branch 2 not taken.
 293500 auto [x, y] = _data.front();
219
1/2
✓ Branch 1 taken 293500 times.
✗ Branch 2 not taken.
 293500 _data.pop_front();
220
221
2/2
✓ Branch 0 taken 571 times.
✓ Branch 1 taken 292929 times.
 293500 if (_strategy == Strategy::IncrementalLeastSquares)
222 {
223
1/2
✓ Branch 1 taken 571 times.
✗ Branch 2 not taken.
 571 _incrementalLSQ.removeDataPoint(x, y);
224 }
225 }
226 };
227
228 /// @brief Converts the enum to a string
229 /// @param[in] strategy Enum value to convert into text
230 /// @return String representation of the enum
231 const char* to_string(PolynomialRegressor<>::Strategy strategy);
232
233 /// @brief Converts the provided object into json
234 /// @param[out] j Json object which gets filled with the info
235 /// @param[in] obj Object to convert into json
236 template<typename Scalar = double>
237 void to_json(json& j, const PolynomialRegressor<Scalar>& obj)
238 {
239 j = json{
240 { "strategy", obj._strategy },
241 { "polyDegree", obj._polyDegree },
242 { "windowSize", obj._windowSize },
243 };
244 }
245
246 /// @brief Converts the provided json object into a node object
247 /// @param[in] j Json object with the needed values
248 /// @param[out] obj Object to fill from the json
249 template<typename Scalar = double>
250 void from_json(const json& j, PolynomialRegressor<Scalar>& obj)
251 {
252 if (j.contains("strategy"))
253 {
254 j.at("strategy").get_to(obj._strategy);
255 }
256 if (j.contains("polyDegree"))
257 {
258 j.at("polyDegree").get_to(obj._polyDegree);
259 obj._incrementalLSQ.setPolynomialDegree(obj._polyDegree);
260 }
261 if (j.contains("windowSize"))
262 {
263 j.at("windowSize").get_to(obj._windowSize);
264 }
265 }
266
267 } // namespace NAV
268