点云多项式拟合（Z拟合为X和Y的二元n阶多项式）

double predictZ(const Eigen::VectorXd& coeffs, double x, double y, int order) {
    double z = 0;
    int idx = 0;
    for (int dx = 0; dx <= order; ++dx) {
        for (int dy = 0; dy <= dx; ++dy) {
            z += coeffs[idx++] * std::pow(x, dx - dy) * std::pow(y, dy);
        }
    }
    return z;
}

double computeRMSE(const std::vector<double>& X, const std::vector<double>& Y,
	const std::vector<double>& Z, const Eigen::VectorXd& coeffs, int order) {
	double error = 0;
	for (size_t i = 0; i < X.size(); ++i) {
		double diff = Z[i] - predictZ(coeffs, X[i], Y[i], order);
		error += diff * diff;
	}
	return std::sqrt(error / X.size());
}

// 计算设计矩阵 A 和目标向量 b
void computeDesignMatrixAndVector(const std::vector<double>& X, 
    const std::vector<double>& Y,
	const std::vector<double>& Z, int order, 
    Eigen::MatrixXd& A, Eigen::VectorXd& b)
{
	int n = X.size();
	A.resize(n, (order + 2) * (order + 1) / 2); // 二阶多项式有 6 个参数
	b.resize(n);

	for (int i = 0; i < n; ++i) {
		double x = X[i];
		double y = Y[i];
		double z = Z[i];

        int id = 0;
		for (int dx = 0; dx <= order; ++dx) {
			for (int dy = 0; dy <= dx; ++dy) {
				A(i, id++) = std::pow(x, dx - dy) * std::pow(y, dy);
			}
		}
		b(i) = z;
	}
}

void main()
{
	// --------------------- 1. 生成测试数据 ---------------------
	std::vector<double> true_coeffs;
	int len = (order + 1) * (order + 2) / 2;
	true_coeffs.resize(len);
	for (int i = 0;i < len;i++)
	{
		true_coeffs[i] = rand() % 100 / 100.0 * 3.0;
	}

	std::vector<double> X, Y, Z;
	for (double x = -20; x <= 20; x += 0.2) {
		for (double y = -20; y <= 20; y += 0.2) {
			X.push_back(x);
			Y.push_back(y);
			// 计算真实值并添加噪声			
			double z_true = 0;
			int id = 0;
			for (int dx = 0; dx <= order; ++dx) {
				for (int dy = 0; dy <= dx; ++dy) {
					z_true+= true_coeffs[id] * std::pow(x, dx - dy) * std::pow(y, dy);
					id++;
				}
			}
            
            double noise = 0.01 * (rand() % 100 - 50) / 50.0;  // [-0.1, 0.1] 的噪声
				Z.push_back(z_true + noise);
			}
		}
		Eigen::MatrixXd A;
		Eigen::VectorXd b;
		computeDesignMatrixAndVector(X, Y, Z, order, A, b);

		// 使用最小二乘法求解
		Eigen::VectorXd coefficients = A.colPivHouseholderQr().solve(b);
		std::cout << "Coefficients: " << coefficients.transpose() << std::endl;
       
		// 计算 RMSE
		double rmse = computeRMSE(X, Y, Z, coefficients, order);
		std::cout << "\nRMSE: " << rmse << std::endl;
	}