Encoder Interpolation

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <vector>
#include <Eigen/QR>
#include <unistd.h>
#include <getopt.h>

bool lVerbose = false;

void usage(const char *name)
{
    printf("usage: %s [-r <ratio>] [-v] val1 val2 val3 [val4 ...]\n", name);
}

void polyfit(const std::vector<double> &t,
             const std::vector<double> &v,
             std::vector<double> &coeff,
             int order

             )
{
    static bool firstCall = true;

    // Create Matrix Placeholder of size n x k, n= number of datapoints, k = order of polynomial, for exame k = 3 for cubic polynomial
    Eigen::MatrixXd T(t.size(), order + 1);
    Eigen::VectorXd V = Eigen::VectorXd::Map(&v.front(), v.size());
    Eigen::VectorXd result;

    // check to make sure inputs are correct
    assert(t.size() == v.size());
    assert(t.size() >= order + 1);
    if (firstCall && lVerbose) {
        printf("t.size() = %zu\n", t.size());
        printf("v.size() = %zu\n", v.size());
        firstCall = false;
    }
    // Populate the matrix
    for(size_t i = 0 ; i < t.size(); ++i)
        {
            for(size_t j = 0; j < order + 1; ++j)
                {
                    T(i, j) = pow(t.at(i), j);
                }
        }
    //std::cout<<T<<std::endl;
    
    // Solve for linear least square fit
    result  = T.householderQr().solve(V);
    coeff.resize(order+1);
    for (int k = 0; k < order+1; k++)
        {
            coeff[k] = result[k];
        }

}

int main(int argc, char **argv)
{
    int c;
    unsigned long ratio = 10;
    int order = 2;

    while ((c = getopt(argc, argv, "r:o:vh")) != EOF) {
        switch(c) {
            case 'r':
                ratio = std::stoul(optarg);
                break;
            case 'o':
                order = std::stoul(optarg);
                break;
            case 'v':
                lVerbose = true;
                break;
            case 'h':
                usage(argv[0]);
                return 0;
            default:
                return 1;
        }
    }

    int input_count = argc - optind;
    if (input_count < 3) {
        printf("usage: %s [-r <ratio>] [-o <order>] [-v] val1 val2 val3 [val4 ...]\n", argv[0]);
        return 1;
    }

    if (lVerbose) {
        printf("ratio: %u\n", ratio);
    }

    std::vector<double> time;
    std::vector<double> velocity;
    for (int index = optind; index < argc; index++) {
        if (lVerbose) {
            printf ("Non-option argument %s\n", argv[index]);
        }
        double increment = (1.0 / (double) (input_count - 1));
        time.push_back((index - optind) * increment);
        velocity.push_back(std::stoul(argv[index]));
    }

    // placeholder for storing polynomial coefficient
    std::vector<double> coeff;
    polyfit(time, velocity, coeff, order);
    if (lVerbose) {
        printf("coeff.size = %u\n", coeff.size());
        std::cout<< "Printing fitted values (q)" << std::endl;
    }

    double vfitted;
    unsigned fast_index;
    for(unsigned fast_index = 0; fast_index < (input_count - 1) * ratio + 1; fast_index ++) {
        if (fast_index > 0) {
            std::cout << ",";
        }
        double q = ((double) fast_index / (ratio * (input_count - 1)));
        vfitted = coeff[0];
        for (int ord = 1; ord <= order; ord++) {
            vfitted += (coeff[ord] * pow(q, ord));
        }

        // if fitted value is less than zero then set it to 0
        if (vfitted < 0.0) {
            vfitted = 0.0;
        }

        // if fitted value is greater than 0xffffffff then set it to 0xffffffff
        if (vfitted > 0xffffffff) {
            vfitted = 0xffffffff;
        }

        std::cout  <<std::fixed << vfitted;
    }
    std::cout<<std::endl;

    return 0;
}

ifndef GXX
$(error GXX must be set in environment)
endif

ifndef CONDA_PREFIX
$(error CONDA_PREFIX must be set in environment)
endif

eigen: eigen.cc
$(GXX) eigen.cc -o eigen -I $(CONDA_PREFIX)/include/eigen3
polyfit.py

#!/usr/bin/env python

import matplotlib.pyplot as plt
import argparse
import subprocess
import sys

def main():
    parser = argparse.ArgumentParser(prog="polyfit", usage="%(prog)s [options] val val val [val...]")
    parser.add_argument('val', nargs='*', help='3 or more values')
    parser.add_argument('-r', type=int, default=10, help='trigger rate ratio (default=10)')
    parser.add_argument('-o', type=int, default=2, help='order of polynomial (default=2)')
    parser.add_argument('-v', action='store_true', help='be verbose')
    args = parser.parse_args()
    ratio = args.r
    order = args.o

    cmd = ["./eigen", "-o", f"{order}", "-r", f"{ratio}"] + args.val

    if len(args.val) < 3:
        parser.error("3 or more values required")

    if args.v:
        print(f"cmd: {cmd}")

    xx = subprocess.run(cmd, capture_output=True)

    result = xx.stdout.decode('UTF-8').strip()

    res = [ float(x) for x in result.split(",") ]
    if args.v:
        print(f"ratio: {ratio}")

    slow_times = range(0, ratio * len(args.val), ratio)
    fast_times = range(0, len(res), 1)

    if args.v:
        print(f"slow_times: (len {len(slow_times)}):  {slow_times}")
        print(f"args.val:   (len {len(args.val)}):    {args.val}")
        print(f"fast_times: (len {len(fast_times)}):  {fast_times}")
        print(f"res:        (len {len(res)}):         {res}")

    intval = [int(v) for v in args.val]
    plt.plot(slow_times, intval, 'ro', markersize=12.0)
    plt.plot(fast_times, res, 'g^')
    plt.title(f'Interpolated Encoder Values  (order={order})')
    plt.grid(True)
    plt.show()

if __name__ == '__main__':
    main()