本文首发于公众号【DeepDriving】，欢迎关注，代码获取方式见文末。

前言

在自动驾驶系统中，通常会用起点、终点和一个三阶多项式来表示一条车道线，多项式系数的求解一般用最小二乘法来实现。

本文首先介绍两种基于最小二乘法的多项式拟合方法的原理，然后基于OpenCV用c++编写了这两种拟合方法的代码，最后通过一个完整的示例来展示如何通过一个离散点集拟合出一条多项式曲线。

基于最小二乘法的多项式拟合原理推导

代数方式求解

多项式曲线拟合是指基于一系列的观测点去寻找一个多项式来表示这些点的关系，最小二乘法通过最小化误差的平方和去寻找数据的最佳匹配函数。假设有点集

P = { ( x 1 ， y 1 ) , ( x 2 ， y 2 ) , … , ( x n ， y n ) } P=\left \{ (x_{1}，y_{1}),(x_{2}，y_{2}), \dots ,(x_{n}，y_{n})\right \} P={(x1​，y1​),(x2​，y2​),…,(xn​，yn​)}

其中，$x_i\in R$和$y_i\in R$的关系满足函数

$$f(x_i)=y_i,i=1,2,\dots ,n$$

假设$m$次多项式函数为

$$\begin{array}{rl}f(x_i,w_j)& =w_0+w_1{x}_i+w_2{x}_i^2+\cdots +w_m{x}_i^m\\ & =\sum _{j=0}^m{w}_j{x}_i^j\end{array}$$

其中$w_j$为多项式系数。如果要用这个$m$次多项式来表示$x$与$y$的关系，那么多项式值与真实值之间的误差为

$$\begin{array}{rl}{e}_i& =f(x_i)-f(x_i,w_j)\\ & =y_i-\sum _{j=0}^m{w}_j{x}_i^j\end{array}$$

采用最小二乘法进行多项式拟合的目的就是寻找一组最佳的多项式系数使得拟合后整个点集的总误差最小，而求总误差最小的问题可以转化为求误差平方和最小。整个点集的误差平方和为

$$E(w)=\sum _{i=1}^n(\sum _{j=0}^m(w_j{x}_i^j-y_i){)}^2$$

要使$E(w)$最小，可以对$w_k$($k=0,1,2,\dots ,m$)求偏导并令其为零：

$$\frac{\partial E(w)}{w_k}=2\sum _{i=1}^n\sum _{j=0}^m((w_j{x}_i^j-y_i)x_i^k)=0$$

可得

$$\begin{gathered} \sum _{i=1}^n\sum _{j=0}^m{w}_j{x}_i^{j+k}=\sum _{i=1}^n{x}_i^k{y}_i \\ k=0,1,2,\dots ,m \end{gathered}$$

写成矩阵形式可得

$$\left[\begin{array}{ccccc}n& {\sum }_{i=1}^n{x}_i& {\sum }_{i=1}^n{x}_i^2& \dots & {\sum }_{i=1}^n{x}_i^m\\ {\sum }_{i=1}^n{x}_i& {\sum }_{i=1}^n{x}_i^2& {\sum }_{i=1}^n{x}_i^3& \dots & {\sum }_{i=1}^n{x}_i^{m+1}\\ {\sum }_{i=1}^n{x}_i^2& {\sum }_{i=1}^n{x}_i^3& {\sum }_{i=1}^n{x}_i^4& \dots & {\sum }_{i=1}^n{x}_i^{m+2}\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ {\sum }_{i=1}^n{x}_i^m& {\sum }_{i=1}^n{x}_i^{m+1}& {\sum }_{i=1}^n{x}_i^{m+2}& \dots & {\sum }_{i=1}^n{x}_i^{2m}\end{array}\right]\left[\begin{array}{c}{w}_0\\ w_1\\ w_2\\ ⋮\\ w_m\end{array}\right]=\left[\begin{array}{c}{\sum }_{i=1}^n{y}_i\\ {\sum }_{i=1}^n{x}_i{y}_i\\ {\sum }_{i=1}^n{x}_i^2{y}_i\\ ⋮\\ {\sum }_{i=1}^n{x}_i^m{y}_i\end{array}\right]$$

把上式写为$AW=B$，那么只要计算出

$$\sum _{i=1}^n{x}_i^j,j=0,1,2,\dots ,2m$$

和

$$\sum _{i=1}^n{x}_i^j{y}_i,j=0,1,2,\dots ,m$$

，再代入上面的式子中构建出矩阵$A$和$B$，那么多项式系数$W$就可以通过求解线性方程组得到。

矩阵方式求解

从上一节的推导过程可以看到，用代数法求解多项式系数的方法非常繁琐而且计算量比较大，我们其实还可以用矩阵求解的方法来简化计算过程。

令

$$A=\left[\begin{array}{ccccc}1& x_1& x_1^2& \dots & x_1^m\\ 1& x_2& x_2^2& \dots & x_2^m\\ 1& x_3& x_3^2& \dots & x_3^m\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ 1& x_n& x_n^2& \dots & x_n^m\end{array}\right],W=\left[\begin{array}{c}{w}_0\\ w_1\\ w_2\\ ⋮\\ w_m\end{array}\right],B=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\\ ⋮\\ y_n\end{array}\right]$$

那么整个点集的误差平方和可以表示为

$$\begin{array}{rl}E& =(AW-B{)}^T(AW-B)\\ & =(W^T{A}^T-B^T)(AW-B)\\ & =W^T{A}^{T}AW-B^{T}AW-W^T{A}^{T}B+B^{T}B\end{array}$$

对$W$求导可得

$$\begin{array}{rl}\frac{\partial E}{\partial W}& =A^{T}AW+W^T{A}^{T}A-A^{T}B-A^{T}B+0\\ & =2A^{T}AW-2A^{T}B\end{array}$$

令导数为零，那么可以得到

$$A^{T}AW=A^{T}B$$

解得

$$W=(A^{T}A{)}^{-1}{A}^{T}B$$

上面求导过程中用到了几个矩阵求导的性质：

$$\frac{\partial x^{T}ax}{\partial x}=ax+a^{T}x$$

$$\frac{\partial x^{T}a}{\partial x}=\frac{\partial a^{T}x}{\partial x}=a$$

2. 如果$a$是对称矩阵，那么

$$ax+a^{T}x=2ax$$

所以有

$$\frac{\partial W^T{A}^{T}AW}{\partial W}=A^{T}AW+W^T{A}^{T}A=2A^{T}AW$$

$$\frac{\partial B^{T}AW}{\partial W}=\frac{\partial W^T{A}^{T}B}{\partial W}=A^{T}B$$

多项式曲线拟合的C++代码实现

学习了前面的理论知识，我们用c++来编码实现一下，毕竟光看一堆数学公式还是不够直观。从前面的理论知识可以知道，用最小二乘法做多项式曲线拟合其实质就是一个矩阵求解的问题，我们可以借助一些常用的数学库比如OpenCV或Eigen来求解。本文将采用OpenCV来实现。

1. 代数方式求解的C++代码实现

如果理解了前面的理论，写代码其实就比较简单了。对照前面理论部分的公式，构建好矩阵A和B，然后调用OpenCV的solve()函数来求解即可：

// 代数方式求解
void PolyFit(const std::vector<cv::Point2f> &points, const int order,
             cv::Mat *coeff) {
  const int n = points.size();
  cv::Mat A = cv::Mat::zeros(order + 1, order + 1, CV_64FC1);
  cv::Mat B = cv::Mat::zeros(order + 1, 1, CV_64FC1);

  // 构建A矩阵
  for (int i = 0; i < order + 1; ++i) {
    for (int j = 0; j < order + 1; ++j) {
      for (int k = 0; k < n; ++k) {
        A.at<double>(i, j) += std::pow(points.at(k).x, i + j);
      }
    }
  }

  // 构建B矩阵
  for (int i = 0; i < order + 1; ++i) {
    for (int k = 0; k < n; ++k) {
      B.at<double>(i, 0) += std::pow(points.at(k).x, i) * points.at(k).y;
    }
  }

  (*coeff) = cv::Mat::zeros(order + 1, 1, CV_64FC1);
  // 求解
  if (!cv::solve(A, B, *coeff, cv::DECOMP_LU)) {
    std::cout << "Failed to solve !" << std::endl;
  }
}

2. 矩阵方式求解的C++代码实现

矩阵方式求解的代码更加简单：

// 矩阵方式求解
void PolyFit(const std::vector<cv::Point2f> &points, const int order,
             cv::Mat *coeff) {
  const int n = points.size();
  cv::Mat A = cv::Mat::ones(n, order + 1, CV_64FC1);
  cv::Mat B = cv::Mat::zeros(n, 1, CV_64FC1);

  for (int i = 0; i < n; ++i) {
    const double a = points.at(i).x;
    const double b = points.at(i).y;
    B.at<double>(i, 0) = b;

    for (int j = 0, v = 1.0; j < order + 1; ++j, v *= a) {
      A.at<double>(i, j) = v;
    }
  }

  // 使用cv::solve求解(A^T * A) * coeff = A^T * B
  cv::Mat At = A.t();
  cv::Mat AtA = At * A;
  cv::Mat AtB = At * B;
  cv::solve(AtA, AtB, *coeff, cv::DECOMP_NORMAL);
}

3. 一个完整的多项式曲线拟合示例

接下来，我们来看一个简单的曲线拟合示例：

int main(int argc, char **argv) {
  constexpr int kOrder = 3; // 多项式阶数
  constexpr int kWidth = 1000;
  constexpr int kHeight = 500;

  cv::Mat canvas =
      cv::Mat(cv::Size(kWidth, kHeight), CV_8UC3, cv::Scalar(255, 255, 255));

  cv::RNG rng(0xFFFFFFFF); // 随机数

  // 生成点集，y坐标添加一些随机噪声
  std::vector<cv::Point2f> raw_points;
  for (int i = 10; i < kWidth; i += 10) {
    cv::Point2f p;
    p.x = i;
    const auto noise = rng.uniform(-kHeight / 10, kHeight / 10);
    p.y = kHeight - p.x / kWidth * kHeight + noise;

    cv::circle(canvas, p, 5, cv::Scalar(0, 0, 255), -1);
    raw_points.emplace_back(p);
  }

  // 多项式拟合
  cv::Mat coeff;
  PolyFit(raw_points, kOrder, &coeff);

  // 用拟合后的系数重新生成点集
  std::vector<cv::Point> poly_points;
  for (const auto &rp : raw_points) {
    cv::Point p;
    p.x = rp.x;
    p.y = PolyValue(coeff, kOrder, rp.x);

    cv::circle(canvas, p, 5, cv::Scalar(0, 255, 0), -1);
    poly_points.emplace_back(p);
  }
  
  cv::polylines(canvas, poly_points, false, cv::Scalar(0, 255, 0), 3,
                cv::LINE_4);

  cv::imshow("PolyFit", canvas);
  cv::waitKey(0);
  cv::destroyAllWindows();

  return 0;
}

代码的流程如下：

1. 生成一个离散点集，其中每个点的y坐标被添加一定的随机噪声；
2. 调用前面实现的多项式拟合函数求解多项式系数；
3. 用求解出的多项式系数重新计算每个点的y坐标值；
4. 可视化原始点集和拟合后的点集。

其中用于计算多项式值的函数PolyValue()实现如下：

float PolyValue(const cv::Mat &coeff, const int order, const float x) {
  float v = 0;
  for (int i = 0; i < order; ++i) {
    v += coeff.at<double>(i, 0) * std::pow(x, i);
  }
  return v;
}

可视化的结果如下图所示，其中红色点为原始点，绿色点为拟合后的点。

总结

本文详细介绍了基于最小二乘法的多项式拟合的原理和代码实现，希望对各位读者有所帮助。如果对本文内容有疑问，欢迎评论区留言与我交流。

如果需要本文代码，麻烦关注我的公众号【DeepDriving】，后台回复关键字【多项式拟合】获取。