PolynomialRegressor.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <cstddef>
#include <utility>
 
#include <nlohmann/json.hpp>
using json = nlohmann::json; 
 
#include "util/Assert.h"
#include "util/Container/ScrollingBuffer.hpp"
 
#include "Polynomial.hpp"
#include "internal/PolynomialRegressor/IncrementalLeastSquares.hpp"
#include "internal/PolynomialRegressor/LeastSquares.hpp"
#include "internal/PolynomialRegressor/HouseholderQr.hpp"
#include "internal/PolynomialRegressor/BDCSVD.hpp"
#include "internal/PolynomialRegressor/COD.hpp"
 
namespace NAV
{
 
template<typename Scalar = double>
class PolynomialRegressor
{
  public:
    enum class Strategy : uint8_t
    {
        IncrementalLeastSquares, 
        LeastSquares,            
        HouseholderQR,           
        BDCSVD,                  
        COD,                     
        COUNT,                   
    };
 
    PolynomialRegressor(size_t polynomialDegree, size_t windowSize, Strategy strategy = Strategy::HouseholderQR)
        : _strategy(strategy), _polyDegree(polynomialDegree), _windowSize(windowSize), _incrementalLSQ(polynomialDegree)
    {
        setWindowSize(windowSize);
        setPolynomialDegree(polynomialDegree);
    }
 
    void setWindowSize(size_t windowSize)
    {
        INS_ASSERT_USER_ERROR(windowSize > _polyDegree, "The window size needs to be greater than the polynomial degree.");
 
        while (windowSize < _windowSize)
        {
            pop_front();
            _windowSize--;
        }
 
        _windowSize = windowSize;
        _data.resize(windowSize);
    }
 
    void setPolynomialDegree(size_t polynomialDegree)
    {
        INS_ASSERT_USER_ERROR(polynomialDegree < _windowSize, "The polynomial degree needs to be smaller than the window size.");
        _polyDegree = polynomialDegree;
 
        _incrementalLSQ.setPolynomialDegree(polynomialDegree);
 
        reset();
    }
 
    void setStrategy(Strategy strategy)
    {
        INS_ASSERT_USER_ERROR(strategy != Strategy::COUNT, "You cannot call this function with COUNT strategy");
 
        _strategy = strategy;
        reset();
    }
 
    void push_back(const std::pair<Scalar, Scalar>& dataPoint)
    {
        push_back(dataPoint.first, dataPoint.second);
    }
 
    void push_back(const Scalar& x, const Scalar& y)
    {
        if (!_data.empty() && _data.back().first == x)
        {
            if (_strategy == Strategy::IncrementalLeastSquares)
            {
                _incrementalLSQ.removeDataPoint(_data.back().first, _data.back().second);
                _incrementalLSQ.addDataPoint(x, y);
            }
            _data.back().second = y;
 
            return;
        }
 
        if (_data.full()) { pop_front(); }
 
        if (_strategy == Strategy::IncrementalLeastSquares)
        {
            _incrementalLSQ.addDataPoint(x, y);
        }
 
        _data.push_back(std::make_pair(x, y));
    }
 
    void reset()
    {
        _incrementalLSQ.reset();
        _data.clear();
    }
 
    [[nodiscard]] Polynomial<Scalar> calcPolynomial() const
    {
        auto prepareDataVectors = [&]() {
            auto n = static_cast<int>(_data.size());
            Eigen::VectorX<Scalar> x = Eigen::VectorX<Scalar>(n);
            Eigen::VectorX<Scalar> y = Eigen::VectorX<Scalar>(n);
 
            for (size_t i = 0; i < _data.size(); i++)
            {
                x(static_cast<int>(i)) = _data.at(i).first;
                y(static_cast<int>(i)) = _data.at(i).second;
            }
 
            return std::make_pair(x, y);
        };
 
        switch (_strategy)
        {
        case Strategy::IncrementalLeastSquares:
            return { _incrementalLSQ.calcCoefficients() };
        case Strategy::LeastSquares:
        {
            auto [x, y] = prepareDataVectors();
            return { LeastSquares<Scalar>::calcCoefficients(x, y, _polyDegree) };
        }
        case Strategy::HouseholderQR:
        {
            auto [x, y] = prepareDataVectors();
            return { HouseholderQr<Scalar>::calcCoefficients(x, y, _polyDegree) };
        }
        case Strategy::BDCSVD:
        {
            auto [x, y] = prepareDataVectors();
            return { BDCSVD<Scalar>::calcCoefficients(x, y, _polyDegree) };
        }
        case Strategy::COD:
        {
            auto [x, y] = prepareDataVectors();
            return { COD<Scalar>::calcCoefficients(x, y, _polyDegree) };
        }
        case Strategy::COUNT:
            break;
        }
        return { Eigen::VectorX<Scalar>() };
    }
 
    [[nodiscard]] bool windowSizeReached() const
    {
        return _data.size() == _windowSize;
    }
 
    [[nodiscard]] bool empty() const { return _data.empty(); }
 
    [[nodiscard]] const ScrollingBuffer<std::pair<Scalar, Scalar>>& data() const { return _data; }
 
  private:
    Strategy _strategy = Strategy::IncrementalLeastSquares;
    size_t _polyDegree = 2;
    size_t _windowSize = 10;
    ScrollingBuffer<std::pair<Scalar, Scalar>> _data;
    IncrementalLeastSquares<Scalar> _incrementalLSQ;
 
    void pop_front()
    {
        if (_data.empty()) { return; }
 
        auto [x, y] = _data.front();
        _data.pop_front();
 
        if (_strategy == Strategy::IncrementalLeastSquares)
        {
            _incrementalLSQ.removeDataPoint(x, y);
        }
    }
};
 
const char* to_string(PolynomialRegressor<>::Strategy strategy);
 
template<typename Scalar = double>
void to_json(json& j, const PolynomialRegressor<Scalar>& obj)
{
    j = json{
        { "strategy", obj._strategy },
        { "polyDegree", obj._polyDegree },
        { "windowSize", obj._windowSize },
    };
}
 
template<typename Scalar = double>
void from_json(const json& j, PolynomialRegressor<Scalar>& obj)
{
    if (j.contains("strategy"))
    {
        j.at("strategy").get_to(obj._strategy);
    }
    if (j.contains("polyDegree"))
    {
        j.at("polyDegree").get_to(obj._polyDegree);
        obj._incrementalLSQ.setPolynomialDegree(obj._polyDegree);
    }
    if (j.contains("windowSize"))
    {
        j.at("windowSize").get_to(obj._windowSize);
    }
}
 
} // namespace NAV