0.4.1
Loading...
Searching...
No Matches
PolynomialRegressor.hpp
Go to the documentation of this file.
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>
20using json = nlohmann::json; ///< json namespace
21
22#include "util/Assert.h"
24
25#include "Polynomial.hpp"
31
32namespace NAV
33{
34
35/// @brief Polynomial Curve Fitting
36/// @tparam Scalar Data type to store
37template<typename Scalar = double>
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 PolynomialRegressor(size_t polynomialDegree, size_t windowSize, Strategy strategy = Strategy::HouseholderQR)
57 : _strategy(strategy), _polyDegree(polynomialDegree), _windowSize(windowSize), _incrementalLSQ(polynomialDegree)
58 {
59 setWindowSize(windowSize);
60 setPolynomialDegree(polynomialDegree);
61 }
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 void setWindowSize(size_t windowSize)
66 {
67 INS_ASSERT_USER_ERROR(windowSize > _polyDegree, "The window size needs to be greater than the polynomial degree.");
68
69 while (windowSize < _windowSize)
70 {
71 pop_front();
73 }
74
75 _windowSize = windowSize;
76 _data.resize(windowSize);
77 }
78
79 /// @brief Set the Polynomial Degree and resets the data
80 /// @param[in] polynomialDegree Degree of the polynomial to fit
81 void setPolynomialDegree(size_t polynomialDegree)
82 {
83 INS_ASSERT_USER_ERROR(polynomialDegree < _windowSize, "The polynomial degree needs to be smaller than the window size.");
84 _polyDegree = polynomialDegree;
85
86 _incrementalLSQ.setPolynomialDegree(polynomialDegree);
87
88 reset();
89 }
90
91 /// @brief Set the strategy for the fit and resets the data
92 /// @param strategy Strategy to use for fitting data
93 void setStrategy(Strategy strategy)
94 {
95 INS_ASSERT_USER_ERROR(strategy != Strategy::COUNT, "You cannot call this function with COUNT strategy");
96
97 _strategy = strategy;
98 reset();
99 }
100
101 /// @brief Add a data point to the polynomial
102 /// @param[in] dataPoint Data point
103 void push_back(const std::pair<Scalar, Scalar>& dataPoint)
104 {
105 push_back(dataPoint.first, dataPoint.second);
106 }
107
108 /// @brief Add a data point to the polynomial
109 /// @param[in] x X Value
110 /// @param[in] y Y Value
111 void push_back(const Scalar& x, const Scalar& y)
112 {
113 if (!_data.empty() && _data.back().first == x)
114 {
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 if (_data.full()) { pop_front(); }
126
128 {
129 _incrementalLSQ.addDataPoint(x, y);
130 }
131
132 _data.push_back(std::make_pair(x, y));
133 }
134
135 /// @brief Reset the polynomial coefficients and saved data
136 void reset()
137 {
138 _incrementalLSQ.reset();
139 _data.clear();
140 }
141
142 /// @brief Calculates the polynomial
143 [[nodiscard]] Polynomial<Scalar> calcPolynomial() const
144 {
145 auto prepareDataVectors = [&]() {
146 auto n = static_cast<int>(_data.size());
147 Eigen::VectorX<Scalar> x = Eigen::VectorX<Scalar>(n);
148 Eigen::VectorX<Scalar> y = Eigen::VectorX<Scalar>(n);
149
150 for (size_t i = 0; i < _data.size(); i++)
151 {
152 x(static_cast<int>(i)) = _data.at(i).first;
153 y(static_cast<int>(i)) = _data.at(i).second;
154 }
155
156 return std::make_pair(x, y);
157 };
158
159 switch (_strategy)
160 {
162 return { _incrementalLSQ.calcCoefficients() };
164 {
165 auto [x, y] = prepareDataVectors();
167 }
169 {
170 auto [x, y] = prepareDataVectors();
172 }
173 case Strategy::BDCSVD:
174 {
175 auto [x, y] = prepareDataVectors();
177 }
178 case Strategy::COD:
179 {
180 auto [x, y] = prepareDataVectors();
182 }
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 [[nodiscard]] bool windowSizeReached() const
191 {
192 return _data.size() == _windowSize;
193 }
194
195 /// @brief Checks if the container has no elements
196 [[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
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
210 /// Incremental LSQ Regressor
212
213 /// @brief Removes the first data point from the polynomial fit (sliding window)
215 {
216 if (_data.empty()) { return; }
217
218 auto [x, y] = _data.front();
219 _data.pop_front();
220
222 {
223 _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
231const 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
236template<typename Scalar = double>
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
249template<typename Scalar = double>
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
Assertion helpers.
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
Definition Assert.h:21
Bidiagonal Divide and Conquer SVD Curve Fit.
Complete Orthogonal Decomposition Curve Fit.
nlohmann::json json
json namespace
Least Squares Curve Fit.
Incremental Least Squares Curve Fit.
Polynomial.
A buffer which is overwriting itself from the start when full.
static Eigen::VectorX< Scalar > calcCoefficients(const Eigen::MatrixBase< DerivedX > &x, const Eigen::MatrixBase< DerivedY > &y, size_t polynomialDegree=2)
Calculates the polynomial coefficients in order a0 + a1 * x + a2 * x^2 + ...
Definition BDCSVD.hpp:40
static Eigen::VectorX< Scalar > calcCoefficients(const Eigen::MatrixBase< DerivedX > &x, const Eigen::MatrixBase< DerivedY > &y, size_t polynomialDegree=2)
Calculates the polynomial coefficients in order a0 + a1 * x + a2 * x^2 + ...
Definition COD.hpp:40
static Eigen::VectorX< Scalar > calcCoefficients(const Eigen::MatrixBase< DerivedX > &x, const Eigen::MatrixBase< DerivedY > &y, size_t polynomialDegree=2)
Calculates the polynomial coefficients in order a0 + a1 * x + a2 * x^2 + ...
Incremental Least Squares Curve Fitting.
static Eigen::VectorX< Scalar > calcCoefficients(const Eigen::MatrixBase< DerivedX > &x, const Eigen::MatrixBase< DerivedY > &y, size_t polynomialDegree=2)
Calculates the polynomial coefficients in order a0 + a1 * x + a2 * x^2 + ...
Polynomial Curve Fitting.
IncrementalLeastSquares< double > _incrementalLSQ
PolynomialRegressor(size_t polynomialDegree, size_t windowSize, Strategy strategy=Strategy::HouseholderQR)
Constructor.
const ScrollingBuffer< std::pair< Scalar, Scalar > > & data() const
Gets the underlying buffer.
bool windowSizeReached() const
Checks if the amount of data points equals the window size.
bool empty() const
Checks if the container has no elements.
void push_back(const Scalar &x, const Scalar &y)
Add a data point to the polynomial.
void setStrategy(Strategy strategy)
Set the strategy for the fit and resets the data.
Strategy
Possible Fit strategies.
@ BDCSVD
Bidiagonal Divide and Conquer SVD.
@ IncrementalLeastSquares
Incremental Least Squares (only polynomials of order <= 2)
@ COUNT
Amount of items in the enum.
@ HouseholderQR
Householder QR decomposition.
@ COD
Complete Orthogonal Decomposition.
@ LeastSquares
Least Squares (bas if even mildly ill-conditioned)
void setWindowSize(size_t windowSize)
Sets the amount of points used for the fit (sliding window)
void setPolynomialDegree(size_t polynomialDegree)
Set the Polynomial Degree and resets the data.
void reset()
Reset the polynomial coefficients and saved data.
Polynomial< Scalar > calcPolynomial() const
Calculates the polynomial.
void push_back(const std::pair< Scalar, Scalar > &dataPoint)
Add a data point to the polynomial.
void pop_front()
Removes the first data point from the polynomial fit (sliding window)
ScrollingBuffer< std::pair< double, double > > _data
Polynomial.
A buffer which is overwriting itself from the start when full.
Least Squares Curve Fit.
void to_json(json &j, const Node &node)
Converts the provided node into a json object.
Definition Node.cpp:990
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
void from_json(const json &j, Node &node)
Converts the provided json object into a node object.
Definition Node.cpp:1007