INSTINCT Code Coverage Report

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