【数学和算法】最小二乘法理论(附c++代码)

插值：是给出一些点，求出这些点的表达式，表达式一定经过这些点，然后利用表达式就可以得到某点的值。

插值应用场景：在数据点稀少的时候，也可以用来插值，使得数据点密集。

---

拟合：是给出一些点，求出最接近这些点的曲线表达式，表达式不要求穿过这些点，只要求最贴合这些点，这就是拟合。

拟合应用场景：用来使得这些点的曲线更平滑。

---

注意多项式插值与多项式拟合的区别：

• 多项式插值的话不需要用到最小二乘法，比如三次多项式插值，你只需要四个点，四个方程，组成由4个多项式系数作为未知数的四元一次方程组，就可以求出三次多项式的四个系数；
• 多项式拟合，例如三次多项式拟合，可能会给你几十个几百个上千个点，然后使用最小二乘法，求出最贴合这些点的曲线来拟合这些点。

看了上面的区别，应该很明了了，如果使用多项式插值，每增加一个点，多项式次数就会增加，就需要重新计算，而且次数也并非越大越好，会有龙格现象，在实际工程中，这显然不合理，所以多项式插值一般只是理论中一带而过。实际工程中我们通常使用多项式拟合。当数据点本身很多的话，多项式插值则次数很高，龙格现象会造成不准确插值。当然这一点可以通过分段低次插值克服，而不会直接用到高次数的多项式插值。

---

下面只介绍拟合中最常用的最小二乘法，理论部分直接参考知乎大佬的博客： 最小二乘法

1.最小二乘法 理论部分

知乎大佬的博客中讲到了最小二乘法推导，以及选择算术平均数、一次多项式曲线、二次多项式曲线等进行拟合的效果。

• 如果使用算术平均数来拟合，得到的拟合曲线的所有点的y值都相等，即平行于x轴的直线(斜率为0)；
• 如果使用一次多项式来拟合，得到的拟合曲线的所有点的y值不相等，即一条斜率不为0的直线；
• 如果使用二次多项式来拟合，得到的拟合曲线是一个二次函数的曲线；
• 如果使用n次多项式来拟合，得到的拟合曲线是一个n次函数的曲线；

也就是说，给你一系列可能是曲线的点，你可以选择不同的函数来拟合这些点，得到拟合曲线函数。

注意看，上面两个偏导=0的方程组求解，是把所有样本代入进去进行求和：并不是一个样本代入一个方程，而是所有样本一起代入。

上面就是使用一次多项式曲线，注意，不是二次多项式，因为最小二乘法就是求距离的平方和最小值，所以接了个平方。

---

只需要记住，最小二乘法是用来求多项式的系数，所以求导是分别求平方和对各个系数的偏导，令偏导为0，就得到了方程组，把各个点的(xi,yi)一起代入方程组进行求解，就得到了多项式的系数。不过这个方程组可以使用矩阵的形式A*X=B来求解,利用opencv中的cv::solve函数可以求解。cv::solve函数可参考这篇博客：https://blog.csdn.net/nienelong3319/article/details/80855077

---

下面的截图是推导：
其中公式里面的i=0有误，都改为i=1,因为点是从(x1,y1)开始的。W3改为Wm

---

代码部分

2.1 多项式拟合曲线C++代码：

// 用最小二乘法根据点集拟合多项式系数
// order是设置的多项式次数
// is_x_axis是根据x轴的方向。因为像素坐标系的x,y和车身坐标系的x，y方向不一样
// coeff是求出的系数向量(order+1行，1列）
bool PolyCurveFit(
    const std::vector<Point2D<float>> &point_set, const int &order,
    const bool &is_x_axis, cv::Mat *coeff) {
  if (coeff == nullptr) {
    SERROR << "The pointer of coefficient matrix is null!";
    return false;
  }
  const int n = static_cast<int>(point_set.size());
  if (n <= order) {
    SERROR << "The number of points should be larger than the order."
              "The number of points = "
           << n;
    return false;
  }

  cv::Mat A = cv::Mat::zeros(order + 1, order + 1, CV_64FC1);
  cv::Mat B = cv::Mat::zeros(order + 1, 1, CV_64FC1);

  // Fill matrix A
  for (int i = 0; i < order + 1; ++i) {
    for (int j = 0; j < order + 1; ++j) {
      for (int k = 0; k < n;
           ++k) {  // 遍历点，把每个点的x或y坐标都进行求i+j次方
        // 矩阵A的每个元素都是[x的幂]，或[y的幂],最大幂为2*order
        A.at<double>(i, j) +=
            std::pow(is_x_axis ? static_cast<double>(point_set.at(k).x)
                               : static_cast<double>(point_set.at(k).y),
                     i + j);
      }
    }
  }

  // Fill matrix B
  for (int i = 0; i < order + 1; ++i) {
    // 遍历点，把每个点都进行(x坐标求i次方)*y 或 (y坐标求i次方)*x
    for (int k = 0; k < n; ++k) {
      // 矩阵B的每个元素都是[x的幂*y]，或[y的幂*x]，最大幂为order
      B.at<double>(i, 0) +=
          std::pow(is_x_axis ? static_cast<double>(point_set.at(k).x)
                             : static_cast<double>(point_set.at(k).y),
                   i) *
          (is_x_axis ? static_cast<double>(point_set.at(k).y)
                     : static_cast<double>(point_set.at(k).x));
    }
  }

  (*coeff) = cv::Mat::zeros(order + 1, 1, CV_64FC1);
  // 使用最小二乘法,最后优化问题化简为,求A*X=B, 即 A*coeff=B，
  // 矩阵方程式有了,求未知系数矩阵X,使用的是LU矩阵分解算法
  // 这里就是求解order+1元order次方程组，求得就是系数
  if (!cv::solve(A, B, *coeff, cv::DECOMP_LU)) {
    SERROR << "Cannot solve polynomial coefficient!";
    return false;
  }
  return true;
}

上面代码是使用opencv的库函数解出Ax=b中的x,这里的x就是系数coef。
如果是使用Eigen矩阵的话，那么代码这样的:

const int N = Order + 1;
Eigen::MatrixXd matA(N, N);
Eigen::VectorXd matB(N);

/* 这里省略给矩阵A和向量B赋值 */

//使用LU矩阵分解法求出系数矩阵coef
Eigen::VectorXd coef = matA.lu().solve(matB);

---

2.2 求出一系列点的多项式之后，给出一个点的x值，就可以根据求出的多项式系数，求出该点的y值：

// 模板参数中的N是N次多项式的最高次数，N次多项式有N+1个多项式系数
// std::array<float, N + 1> poly_coeff是通过上面函数求出的多项式系数，可由Mat中转存到array中
// 下面函数就是以 y=a+b*x+c*x^2+d*x^3+...的形式计算x值对应的y值
template <typename T, std::size_t N>
T GetPolyValue(const std::array<float, N + 1> &poly_coeff, T x) {
  T value = 0;
  for (int i = 0; i < N+1; ++i) {
    value += poly_coeff[i] * std::pow(x, i);
  }
  return value;
}

---

2.3 画出x从0～100拟合曲线，x采样间隔为0.3，使用圆圈在图像上画出这些点

• (1) 如果拟合的多项式的点(x,y)本来就是像素坐标系的点,可以直接在图像中画出来:

for (float x = 0; x < 100; x += 0.3) {
  float y = GetPolyValue<float, 3>(poly_coeff, x);
  cv::Point point;
  point.x = x ;
  point.y = y;
  cv::circle(img_, point, 6, cv::Scalar(0, 255, 0));
}

• (2) 如果x，y是车身坐标系的点，那么需要先转化为像素坐标系的点，然后再到图像中标出这些点:

for (float x = 0; x < 100; x += 0.3) {
  float y = GetPolyValue<float, 3>(poly_coeff, x);
  cv::Point point;
  //注意，车身坐标系的x，y和像素坐标系的y,x对应。并且像素坐标系原点在左上角，
  // 即车辆坐标系的x值Xc越大，其对应的像素坐标系Yp值越往上，Yp值就越小
  // 车辆坐标系的y值Yc向左为正，Yc值越大，其对应的像素坐标系的Xp值越小
  point.y = ph0 - static_cast<int>x ;
  point.x = pw0 - static_cast<int>y ;
  cv::circle(img_, point, 6, cv::Scalar(0, 255, 0));
}