多项式参数曲线拟合C++实现

#include <ros/ros.h>
#include <tf/tf.h>
#include <iostream>
#include <map>
#include <math.h>

#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/QR>

// inlcude iostream and string libraries
#include <iostream>
#include <fstream>
#include <string>

using namespace std;
using namespace Eigen;

//获取拟合的参数曲线在x时的y值
double polyeval(Eigen::VectorXd coeffs, double x) 
{
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++) 
    {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

//多项式拟合的一个函数,返回拟合的参数曲线系数
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) 
{
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);

    for (int i = 0; i < xvals.size(); i++)
        A(i, 0) = 1.0;

    for (int j = 0; j < xvals.size(); j++) 
    {
        for (int i = 0; i < order; i++) 
            A(j, i + 1) = A(j, i) * xvals(j);
    }

    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}

int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "eigen_test");
   
    ROS_INFO("Waiting for global path msgs ~");
    int N = 4;
    VectorXd x_veh(N);
    VectorXd y_veh(N);
    x_veh[0] = 0.0;
    y_veh[0] = 0.0;
    x_veh[1] = 1.0;
    y_veh[1] = 1.0;
    x_veh[2] = 2.0;
    y_veh[2] = 1.0;
    x_veh[3] = 3.0;
    y_veh[3] = 0.0;

    // Fit waypoints
    auto coeffs = polyfit(x_veh, y_veh, 3); 
    ROS_INFO("coeffs size:%d",coeffs.size());
    for(int i=0;i < coeffs.size();i++)
    {
        ROS_INFO("coeffs[%d]:%f",i,coeffs[i]);
    }
    ros::waitForShutdown();
    return 0;
}