polyfit.h

Go to the documentation of this file.

 
#ifndef __SABLIB_POLYFIT_H__
#define __SABLIB_POLYFIT_H__
 
#include <stdexcept>
#include <Eigen/Eigen>
 
namespace sablib {

template <typename Derived>
const Eigen::MatrixX<typename Derived::PlainObject::Scalar>
Vandermonde(const Eigen::MatrixBase<Derived> & x, const unsigned int polyorder)
{
    // Although parameters are received as MatrixBase<Derived>, only vector classes are allowed.
    // Others will be rejected at compile time.
    static_assert(Derived::IsVectorAtCompileTime, "Error: x is not vector.");
 
    if(x.size() == 0) {
        throw std::invalid_argument("Vandermonde(): the length of x is zero.");
    }
 
    if(polyorder == 0) {
        throw std::invalid_argument("Vandermonde(): polyorder is zero.");
    }
 
    using Scalar = typename Derived::PlainObject::Scalar;
    unsigned int n = x.size();
    unsigned int m = polyorder + 1;
    Eigen::MatrixX<Scalar> V(n, m);
 
    for(unsigned int i = 0; i < n; ++i) {
        Scalar val = 1.0;
 
        for(unsigned int j = 0; j < m; ++j) {
            V(i, j) = val;
            val *= x(i);
        }
    }
 
    return V;
}

template <typename Derived>
const typename Derived::PlainObject
PolyFit(
    const Eigen::MatrixX<typename Derived::PlainObject::Scalar> & V,
    const Eigen::MatrixBase<Derived> & y
)
{
    // Although parameters are received as MatrixBase<Derived>, only vector classes are allowed.
    // Others will be rejected at compile time.
    static_assert(Derived::IsVectorAtCompileTime, "Error: y is not vector.");
 
    if(V.rows() != y.size()) {
        throw std::invalid_argument("PolyFit(): the sizes of y and V do not match.");
    }
 
    return (V.transpose() * V).ldlt().solve(V.transpose() * y);
}

template <typename Derived>
const typename Derived::PlainObject
PolyFit(const Eigen::MatrixBase<Derived> & x, const Eigen::MatrixBase<Derived> & y, const unsigned int polyorder)
{
    // Although parameters are received as MatrixBase<Derived>, only vector classes are allowed.
    // Others will be rejected at compile time.
    static_assert(Derived::IsVectorAtCompileTime, "Error: x or y is not vector.");
 
    if(x.size() != y.size()) {
        throw std::invalid_argument("PolyFit(): x and y must have the same size.");
    }
 
    if(polyorder + 1 > x.size()) {
        throw std::invalid_argument("PolyFit(): polyorder is too high for the number of points.");
    }
 
    return PolyFit(Vandermonde(x, polyorder), y);
}

template <typename Derived>
const typename Derived::PlainObject
PolyVal(
    const Eigen::MatrixBase<Derived> & coeff,
    const Eigen::MatrixX<typename Derived::PlainObject::Scalar> & V
)
{
    // Although parameters are received as MatrixBase<Derived>, only vector classes are allowed.
    // Others will be rejected at compile time.
    static_assert(Derived::IsVectorAtCompileTime, "Error: coeff is not vector.");
 
    if(coeff.size() == 0) {
        throw std::invalid_argument("PolyVal(): the length of coeff is zero.");
    }
 
    if(V.cols() != coeff.size()) {
        throw std::invalid_argument("PolyVal(): the sizes of V and coeff do not match.");
    }
 
    return V * coeff;
}

template <typename Derived>
const typename Derived::PlainObject
PolyVal(const Eigen::MatrixBase<Derived> & coeff, const Eigen::MatrixBase<Derived> & x)
{
    // Although parameters are received as MatrixBase<Derived>, only vector classes are allowed.
    // Others will be rejected at compile time.
    static_assert(Derived::IsVectorAtCompileTime, "Error: coeff or x is not vector.");
 
    return PolyVal(coeff, Vandermonde(x, coeff.size() - 1));
}

template <typename Derived>
const typename Derived::PlainObject::Scalar
PolyVal(const Eigen::MatrixBase<Derived> & coeff, const typename Derived::PlainObject::Scalar x)
{
    // Although parameters are received as MatrixBase<Derived>, only vector classes are allowed.
    // Others will be rejected at compile time.
    static_assert(Derived::IsVectorAtCompileTime, "Error: coeff is not vector.");
 
    if(coeff.size() == 0) {
        throw std::invalid_argument("PolyVal(): the length of coeff is zero.");
    }
 
    typename Derived::PlainObject::Scalar val = 0, xx = 1.0;
 
    for(int i = 0; i < coeff.size(); i++) {
        val += coeff(i) * xx;
        xx  *= x;
    }
 
    return val;
}
 
}; // namespace sablib
 
#endif // __SABLIB_POLYFIT_H__