【数学和算法】最小二乘法理论(附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 Afor (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*orderA.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 Bfor (int i = 0; i < order + 1; ++i) {// 遍历点，把每个点都进行(x坐标求i次方)*y 或 (y坐标求i次方)*xfor (int k = 0; k < n; ++k) {// 矩阵B的每个元素都是[x的幂*y]，或[y的幂*x]，最大幂为orderB.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));
}