在自動駕駛系統中，通常會用起點、終點和一個三階多項式來表示一條車道線，多項式系數的求解一般用最小二乘法來實現。

本文首先介紹兩種基于最小二乘法的多項式擬合方法的原理，然后基于OpenCV用c++編寫了這兩種擬合方法的代碼，最后通過一個完整的示例來展示如何通過一個離散點集擬合出一條多項式曲線。

多項式曲線擬合是指基于一系列的觀測點去尋找一個多項式來表示這些點的關系，最小二乘法通過最小化誤差的平方和去尋找數據的最佳匹配函數。假設有點集

其中，和的關系滿足函數

假設次多項式函數為

其中為多項式系數。如果要用這個次多項式來表示與的關系，那么多項式值與真實值之間的誤差為

采用最小二乘法進行多項式擬合的目的就是尋找一組最佳的多項式系數使得擬合后整個點集的總誤差最小，而求總誤差最小的問題可以轉化為求誤差平方和最小。整個點集的誤差平方和為

要使最小，可以對()求偏導并令其為零：

可得

寫成矩陣形式可得

把上式寫為，那么只要計算出

和

，再代入上面的式子中構建出矩陣和，那么多項式系數就可以通過求解線性方程組得到。

從上一節的推導過程可以看到，用代數法求解多項式系數的方法非常繁瑣而且計算量比較大，我們其實還可以用矩陣求解的方法來簡化計算過程。

令

那么整個點集的誤差平方和可以表示為

對求導可得

令導數為零，那么可以得到

解得

學習了前面的理論知識，我們用c++來編碼實現一下，畢竟光看一堆數學公式還是不夠直觀。從前面的理論知識可以知道，用最小二乘法做多項式曲線擬合其實質就是一個矩陣求解的問題，我們可以借助一些常用的數學庫比如OpenCV或Eigen來求解。本文將采用OpenCV來實現。

如果理解了前面的理論，寫代碼其實就比較簡單了。對照前面理論部分的公式，構建好矩陣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;
  }
}

矩陣方式求解的代碼更加簡單：

// 矩陣方式求解
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;
    if (i > 0) {
      for (int j = 1, v = a; j < order + 1; ++j, v *= a) {
        A.at<double>(i, j) = v;
      }
    }
  }

  (*coeff) = (A.t() * A).inv() * A.t() * B;
}

接下來，我們來看一個簡單的曲線擬合示例：

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;
}

可視化的結果如下圖所示，其中紅色點為原始點，綠色點為擬合后的點。

本文詳細介紹了基于最小二乘法的多項式擬合的原理和代碼實現，希望對各位讀者有所幫助。如果對本文內容有疑問，歡迎評論區留言與我交流。