Eigen::VectorXf RoadEdgeDetection::fitPolyNomial(const Eigen::VectorXf &xvals, const Eigen::VectorXf &yvals, int order) { assert(xvals.size() == yvals.size()); assert(order >= 1 && order <= xvals.size() - 1); Eigen::MatrixXf 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; }

这段代码实现了多项式拟合算法的具体计算过程。给定点集中的 x 和 y 坐标，以及要拟合的多项式阶数，该函数返回拟合出的多项式系数。具体来说，它将点集中的 x 值和多项式阶数作为参数，构造了一个 (n+1) 行 m 列的矩阵 A，其中 n 是点集大小减一，m 是多项式阶数加一。然后对于矩阵的每一行，依次计算出对应的多项式值，填入矩阵 A 中。最后，通过 QR 分解计算出矩阵 A 的解，即多项式系数。

需要注意的是，该函数假设输入的点集中的 x 值互不相同。如果 x 值存在重复，该函数的计算结果可能会出现问题。