三维空间点 曲线拟合

#include <iostream>
#include <vector>
#include <Eigen/Dense>

// 定义一个三维点的结构体
struct Point3D {
    double x, y, z;
};

// 多项式拟合函数
void polynomialFit(const std::vector<Point3D>& points, int degree, Eigen::VectorXd& coeffsX, Eigen::VectorXd& coeffsY, Eigen::VectorXd& coeffsZ) {
    int n = points.size();
    int numCoeffs = degree + 1;

    Eigen::MatrixXd A(n, numCoeffs);
    Eigen::VectorXd bX(n), bY(n), bZ(n);

    for (int i = 0; i < n; ++i) {
        double t = i / static_cast<double>(n - 1);  // 归一化参数
        for (int j = 0; j < numCoeffs; ++j) {
            A(i, j) = pow(t, j);
        }
        bX(i) = points[i].x;
        bY(i) = points[i].y;
        bZ(i) = points[i].z;
    }

    // 使用最小二乘法求解
    coeffsX = A.colPivHouseholderQr().solve(bX);
    coeffsY = A.colPivHouseholderQr().solve(bY);
    coeffsZ = A.colPivHouseholderQr().solve(bZ);
}

// 计算多项式值
double evaluatePolynomial(const Eigen::VectorXd& coeffs, double t) {
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); ++i) {
        result += coeffs[i] * pow(t, i);
    }
    return result;
}

int main() {
    // 定义一组三维点
    std::vector<Point3D> points = {
        {1.0, 2.0, 3.0},
        {2.0, 3.0, 4.0},
        {3.0, 4.0, 5.0},
        {4.0, 5.0, 6.0},
        {5.0, 6.0, 7.0}
    };

    // 多项式的阶数
    int degree = 2;

    // 拟合的多项式系数
    Eigen::VectorXd coeffsX, coeffsY, coeffsZ;

    // 拟合多项式曲线
    polynomialFit(points, degree, coeffsX, coeffsY, coeffsZ);

    // 输出拟合得到的多项式系数
    std::cout << "X方向多项式系数: " << coeffsX.transpose() << std::endl;
    std::cout << "Y方向多项式系数: " << coeffsY.transpose() << std::endl;
    std::cout << "Z方向多项式系数: " << coeffsZ.transpose() << std::endl;

    // 输出拟合曲线的点
    std::cout << "拟合曲线上的点:" << std::endl;
    for (int i = 0; i < 100; ++i) {
        double t = i / 99.0;
        double x = evaluatePolynomial(coeffsX, t);
        double y = evaluatePolynomial(coeffsY, t);
        double z = evaluatePolynomial(coeffsZ, t);
        std::cout << "(" << x << ", " << y << ", " << z << ")" << std::endl;
    }

    return 0;
}

代码说明
1、定义 Point3D 结构体：
     表示一个三维点，包含 x、y、z 三个坐标。
2、polynomialFit 函数：
     使用多项式拟合给定的三维点集。
     degree 参数指定多项式的阶数。
     coeffsX、coeffsY 和 coeffsZ 分别存储 x、y、z 方向上的多项式系数。
     矩阵 A 包含归一化参数 t 的幂次。
     使用 Eigen 提供的 colPivHouseholderQr().solve 求解最小二乘问题。
3、evaluatePolynomial 函数：
     计算给定多项式在参数 t 处的值。
4、main 函数：
     定义一组三维点并调用 polynomialFit 函数进行多项式曲线拟合。
     输出拟合得到的多项式系数。
     计算并输出拟合曲线上的点。