基于最小二乘法的多项式曲线拟合:从原理到c++实现

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

前言

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

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

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

代数方式求解

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

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={(x1y1),(x2y2),,(xnyn)}

其中, x i ∈ R x_{i}\in R xiR y i ∈ R y_{i}\in R yiR的关系满足函数

f ( x i ) = y i , i = 1 , 2 , … , n f(x_{i})=y_{i}, \quad i=1, 2,\dots , n f(xi)=yi,i=1,2,,n

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

f ( x i , w j ) = w 0 + w 1 x i + w 2 x i 2 + ⋯ + w m x i m = ∑ j = 0 m w j x i j \begin{align*} f(x_{i},w_{j}) &= w_{0} + w_{1}x_{i} + w_{2}x_{i}^{2} + \dots +w_{m}x_{i}^{m} \\ &= \sum_{j=0}^{m} w_{j}x_{i}^{j} \end{align*} f(xi,wj)=w0+w1xi+w2xi2++wmxim=j=0mwjxij

其中 w j w_{j} wj为多项式系数。如果要用这个 m m m次多项式来表示 x x x y y y的关系,那么多项式值与真实值之间的误差为

e i = f ( x i ) − f ( x i , w j ) = y i − ∑ j = 0 m w j x i j \begin{align*} e_{i} &= f(x_{i})-f(x_{i},w_{j}) \\ &= y_{i}-\sum_{j=0}^{m} w_{j}x_{i}^{j} \end{align*} ei=f(xi)f(xi,wj)=yij=0mwjxij

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

E ( w ) = ∑ i = 1 n ( ∑ j = 0 m ( w j x i j − y i ) ) 2 E(w) = \sum_{i=1}^{n} (\sum_{j=0}^{m} (w_{j}x_{i}^{j} - y_{i}))^2 E(w)=i=1n(j=0m(wjxijyi))2

要使 E ( w ) E(w) E(w)最小,可以对 w k w_{k} wk( k = 0 , 1 , 2 , … , m k=0,1, 2,\dots , m k=0,1,2,,m)求偏导并令其为零:

∂ E ( w ) w k = 2 ∑ i = 1 n ∑ j = 0 m ( ( w j x i j − y i ) x i k ) = 0 \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 wkE(w)=2i=1nj=0m((wjxijyi)xik)=0

可得

∑ i = 1 n ∑ j = 0 m w j x i j + k = ∑ i = 1 n x i k y i k = 0 , 1 , 2 , … , m \sum_{i=1}^{n} \sum_{j=0}^{m}w_{j}x_{i}^{j+k} =\sum_{i=1}^{n} x_{i}^{k}y_{i} \\ \quad k=0,1, 2,\dots , m i=1nj=0mwjxij+k=i=1nxikyik=0,1,2,,m

写成矩阵形式可得

[ n ∑ i = 1 n x i ∑ i = 1 n x i 2 … ∑ i = 1 n x i m ∑ i = 1 n x i ∑ i = 1 n x i 2 ∑ i = 1 n x i 3 … ∑ i = 1 n x i m + 1 ∑ i = 1 n x i 2 ∑ i = 1 n x i 3 ∑ i = 1 n x i 4 … ∑ i = 1 n x i m + 2 ⋮ ⋮ ⋮ ⋮ ⋮ ∑ i = 1 n x i m ∑ i = 1 n x i m + 1 ∑ i = 1 n x i m + 2 … ∑ i = 1 n x i 2 m ] [ w 0 w 1 w 2 ⋮ w m ] = [ ∑ i = 1 n y i ∑ i = 1 n x i y i ∑ i = 1 n x i 2 y i ⋮ ∑ i = 1 n x i m y i ] \begin{bmatrix} 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}\\ \vdots & \vdots & \vdots & \vdots & \vdots\\ \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{bmatrix} \begin{bmatrix} w_{0}\\ w_{1}\\ w_{2}\\ \vdots\\ w_{m} \end{bmatrix} = \begin{bmatrix} \sum_{i=1}^{n}y_{i} \\ \sum_{i=1}^{n}x_{i}y_{i} \\ \sum_{i=1}^{n}x_{i}^{2}y_{i}\\ \vdots\\ \sum_{i=1}^{n}x_{i}^{m}y_{i}\\ \end{bmatrix} ni=1nxii=1nxi2i=1nximi=1nxii=1nxi2i=1nxi3i=1nxim+1i=1nxi2i=1nxi3i=1nxi4i=1nxim+2i=1nximi=1nxim+1i=1nxim+2i=1nxi2m w0w1w2wm = i=1nyii=1nxiyii=1nxi2yii=1nximyi

把上式写为 A W = B AW=B AW=B,那么只要计算出

∑ i = 1 n x i j ,   j = 0 , 1 , 2 , … , 2 m \sum_{i=1}^{n}x_{i}^{j}, \ j=0,1,2,\dots, 2m i=1nxij, j=0,1,2,,2m

∑ i = 1 n x i j y i ,   j = 0 , 1 , 2 , … , m \sum_{i=1}^{n}x_{i}^{j}y_{i}, \ j=0,1,2,\dots, m i=1nxijyi, j=0,1,2,,m

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

矩阵方式求解

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

A = [ 1 x 1 x 1 2 … x 1 m 1 x 2 x 2 2 … x 2 m 1 x 3 x 3 2 … x 3 m ⋮ ⋮ ⋮ ⋮ ⋮ 1 x n x n 2 … x n m ] , W = [ w 0 w 1 w 2 ⋮ w m ] , B = [ y 1 y 2 y 3 ⋮ y n ] A=\begin{bmatrix} 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}\\ \vdots & \vdots & \vdots & \vdots & \vdots\\ 1 & x_{n} & x_{n}^{2} & \dots & x_{n}^{m}\\ \end{bmatrix}, \quad W=\begin{bmatrix} w_{0}\\ w_{1}\\ w_{2}\\ \vdots\\ w_{m} \end{bmatrix}, \quad B= \begin{bmatrix} y_{1} \\ y_{2} \\ y_{3}\\ \vdots\\ y_{n}\\ \end{bmatrix} A= 1111x1x2x3xnx12x22x32xn2x1mx2mx3mxnm ,W= w0w1w2wm ,B= y1y2y3yn

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

E = ( A W − B ) T ( A W − B ) = ( W T A T − B T ) ( A W − B ) = W T A T A W − B T A W − W T A T B + B T B \begin{align*} 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{align*} E=(AWB)T(AWB)=(WTATBT)(AWB)=WTATAWBTAWWTATB+BTB

W W W求导可得

∂ E ∂ W = A T A W + W T A T A − A T B − A T B + 0 = 2 A T A W − 2 A T B \begin{align*} \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{align*} WE=ATAW+WTATAATBATB+0=2ATAW2ATB

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

A T A W = A T B A^{T}AW = A^{T}B ATAW=ATB

解得

W = ( A T A ) − 1 A T B W = (A^{T}A)^{-1}A^{T}B W=(ATA)1ATB

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

∂ x T a x ∂ x = a x + a T x \frac{\partial x^{T}ax}{\partial x} = ax+a^{T}x xxTax=ax+aTx

∂ x T a ∂ x = ∂ a T x ∂ x = a \frac{\partial x^{T}a}{\partial x} = \frac{\partial a^{T}x}{\partial x}=a xxTa=xaTx=a

  1. 如果 a a a是对称矩阵,那么

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

所以有

∂ W T A T A W ∂ W = A T A W + W T A T A = 2 A T A W \frac{\partial W^{T}A^{T}AW}{\partial W} = A^{T}AW+W^{T}A^{T}A=2A^{T}AW WWTATAW=ATAW+WTATA=2ATAW

∂ B T A W ∂ W = ∂ W T A T B ∂ W = A T B \frac{\partial B^{T}AW}{\partial W} =\frac{\partial W^{T}A^{T}B}{\partial W}= A^{T}B WBTAW=WWTATB=ATB

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

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

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

如果理解了前面的理论,写代码其实就比较简单了。对照前面理论部分的公式,构建好矩阵AB,然后调用OpenCVsolve()函数来求解即可:

// 代数方式求解
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】,后台回复关键字【多项式拟合】获取。

评论 10
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

DeepDriving

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值