多项式拟合算法(最小二乘法)

CAMEL__

已于 2023-11-18 21:23:51 修改

阅读量127

点赞数

分类专栏：机器人常用算法文章标签：算法

于 2023-11-18 15:49:36 首次发布

本文链接：https://blog.csdn.net/Carzy_/article/details/134479132

版权

机器人常用算法专栏收录该内容

6 篇文章 1 订阅

订阅专栏

多项式拟合算法

背景

给定离散的点进行多项式的拟合

理论

在这里插入图片描述

关于求解方程的推导可以通过投影的方式进行理解，即垂直距离最短，详细推导可以参考"矩阵-力量"P235 页

代码实现

#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

using namespace std;
using namespace Eigen;

const int degree = 5;

void generateData(vector<double>& x, vector<double>& y) {
    for (double i = 0.0; i <= 10.0; i += 0.5) {
        x.push_back(i);
        y.push_back(0.2 * pow(i, 5) - 1.5 * pow(i, 4) + 3.5 * pow(i, 3) + 2.0 * pow(i, 2) - 4.0 * i + 10.0);
    }
}

void polynomialFit(const vector<double>& x, const vector<double>& y, VectorXd& coefficients) {
    MatrixXd A(x.size(), degree + 1);
    VectorXd b(x.size());

    for (int i = 0; i < x.size(); ++i) {
        for (int j = 0; j <= degree; ++j) {
            A(i, j) = pow(x[i], j);
        }
        b(i) = y[i];
    }

    coefficients = (A.transpose() * A).ldlt().solve(A.transpose() * b);
}

void publishData(ros::Publisher& publisher, const vector<double>& x, const vector<double>& y, const VectorXd& coefficients) {
    visualization_msgs::Marker points;
    points.header.frame_id = "base_link";  // Change "base_link" to your desired frame
    points.header.stamp = ros::Time::now();
    points.ns = "points";
    points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = 1.0;
    points.id = 0;
    points.type = visualization_msgs::Marker::POINTS;
    points.scale.x = 0.1;
    points.scale.y = 0.1;
    points.color.r = 1.0;
    points.color.a = 1.0;

    // Original Data
    for (size_t i = 0; i < x.size(); ++i) {
        geometry_msgs::Point p;
        p.x = x[i];
        p.y = y[i];
        points.points.push_back(p);
    }

    // Fitted Data
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "base_link";  // Change "base_link" to your desired frame
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "line";
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = 1;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_strip.scale.x = 0.05;
    line_strip.color.g = 1.0;
    line_strip.color.a = 1.0;

    for (double i = 0.0; i <= 10.0; i += 0.1) {
        geometry_msgs::Point p;
        p.x = i;
        p.y = 0;
        for (int j = degree; j >= 0; --j) {
            p.y += coefficients(j) * pow(i, j);
        }
        line_strip.points.push_back(p);
    }

    publisher.publish(points);
    publisher.publish(line_strip);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "polynomial_fit_visualization");
    ros::NodeHandle nh;

    vector<double> x, y;
    generateData(x, y);

    VectorXd coefficients(degree + 1);
    polynomialFit(x, y, coefficients);

    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("polynomial_fit_marker", 10);

    ros::Rate loop_rate(1);

    while (ros::ok()) {
        publishData(marker_pub, x, y, coefficients);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}