3D 空间中拟合曲线

算法来源：C++/PCL：最小二乘拟合平面直线，平面多项式曲线，空间多项式曲线

以下是我改写的算法，未经验证正确性！！！

// 3D 空间中拟合曲线
void fittingPolynomial3D(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, double &a, double &b, double &c) {
     
    // 获取点的总数
    int num_point = cloud->points.size();
    Eigen::MatrixXd A_(3, 3), B_(3, 1), A123(3, 1);
    double A01(0.0), A02(0.0), A12(0.0), A22(0.0), B00(0.0), B10(0.0), B12(0.0);
    for (int i = 0; i < num_point; ++i) {
     
        double x_y = std::sqrt(std::pow(cloud->points[i].x, 2) + std::pow(cloud->points[i].y, 2));
        A01 += x_y;
        A02 += std::pow(x_y, 2);
        A12 += std::pow(x_y, 3);
        A22 += std::pow(x_y, 4);
        B00 += cloud->points[i].z;
        B10 += x_y * cloud->points[i].z;
        B12 += std::pow(x_y, 2) * cloud->points[i].z;
    }

    A_ << num_point, A01, A02,
          A01, A02, A12,
        A02, A12, A22;
    B_ << B00,
          B10,
          B12;
    A123 = A_.inverse() * B_;
    a = A123(2, 0);
    b = A123(1, 0);
    c = A123(0, 0);

}