使用离散点拟合曲线

参考代码路径:

https://www.bragitoff.com/2015/09/c-program-for-polynomial-fit-least-squares/

https://gist.github.com/Thileban/272a67f2affdc14a5f69ad3220e5c24b

https://blog.csdn.net/jpc20144055069/article/details/103232641

作者源码:

//Polynomial Fit
#include<iostream>
#include<iomanip>
#include<cmath>
using namespace std;
int main()
{int i,j,k,n,N;cout.precision(4);                        //set precisioncout.setf(ios::fixed);cout<<"\nEnter the no. of data pairs to be entered:\n";        //To find the size of arrays that will store x,y, and z valuescin>>N;double x[N],y[N];cout<<"\nEnter the x-axis values:\n";                //Input x-valuesfor (i=0;i<N;i++)cin>>x[i];cout<<"\nEnter the y-axis values:\n";                //Input y-valuesfor (i=0;i<N;i++)cin>>y[i];cout<<"\nWhat degree of Polynomial do you want to use for the fit?\n";cin>>n;                                // n is the degree of Polynomial double X[2*n+1];                        //Array that will store the values of sigma(xi),sigma(xi^2),sigma(xi^3)....sigma(xi^2n)for (i=0;i<2*n+1;i++){X[i]=0;for (j=0;j<N;j++)X[i]=X[i]+pow(x[j],i);        //consecutive positions of the array will store N,sigma(xi),sigma(xi^2),sigma(xi^3)....sigma(xi^2n)}double B[n+1][n+2],a[n+1];            //B is the Normal matrix(augmented) that will store the equations, 'a' is for value of the final coefficientsfor (i=0;i<=n;i++)for (j=0;j<=n;j++)B[i][j]=X[i+j];            //Build the Normal matrix by storing the corresponding coefficients at the right positions except the last column of the matrixdouble Y[n+1];                    //Array to store the values of sigma(yi),sigma(xi*yi),sigma(xi^2*yi)...sigma(xi^n*yi)for (i=0;i<n+1;i++){    Y[i]=0;for (j=0;j<N;j++)Y[i]=Y[i]+pow(x[j],i)*y[j];        //consecutive positions will store sigma(yi),sigma(xi*yi),sigma(xi^2*yi)...sigma(xi^n*yi)}for (i=0;i<=n;i++)B[i][n+1]=Y[i];                //load the values of Y as the last column of B(Normal Matrix but augmented)n=n+1;                //n is made n+1 because the Gaussian Elimination part below was for n equations, but here n is the degree of polynomial and for n degree we get n+1 equationscout<<"\nThe Normal(Augmented Matrix) is as follows:\n";    for (i=0;i<n;i++)            //print the Normal-augmented matrix{for (j=0;j<=n;j++)cout<<B[i][j]<<setw(16);cout<<"\n";}    for (i=0;i<n;i++)                    //From now Gaussian Elimination starts(can be ignored) to solve the set of linear equations (Pivotisation)for (k=i+1;k<n;k++)if (B[i][i]<B[k][i])for (j=0;j<=n;j++){double temp=B[i][j];B[i][j]=B[k][j];B[k][j]=temp;}for (i=0;i<n-1;i++)            //loop to perform the gauss eliminationfor (k=i+1;k<n;k++){double t=B[k][i]/B[i][i];for (j=0;j<=n;j++)B[k][j]=B[k][j]-t*B[i][j];    //make the elements below the pivot elements equal to zero or elimnate the variables}for (i=n-1;i>=0;i--)                //back-substitution{                        //x is an array whose values correspond to the values of x,y,z..a[i]=B[i][n];                //make the variable to be calculated equal to the rhs of the last equationfor (j=0;j<n;j++)if (j!=i)            //then subtract all the lhs values except the coefficient of the variable whose value                                   is being calculateda[i]=a[i]-B[i][j]*a[j];a[i]=a[i]/B[i][i];            //now finally divide the rhs by the coefficient of the variable to be calculated}cout<<"\nThe values of the coefficients are as follows:\n";for (i=0;i<n;i++)cout<<"x^"<<i<<"="<<a[i]<<endl;            // Print the values of x^0,x^1,x^2,x^3,....    cout<<"\nHence the fitted Polynomial is given by:\ny=";for (i=0;i<n;i++)cout<<" + ("<<a[i]<<")"<<"x^"<<i;cout<<"\n";return 0;
}//output attached as .jpg

测试代码

#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>
#include <numeric>//第一种方式
QList<double> discrete_point_fitting_curve(std::vector<cv::Point2d> inPoints, int degreeOfPolynomial) {int numPoints = inPoints.size();std::vector<double> posXs;std::vector<double> posYs;for (int i = 0; i < numPoints; i++){cv::Point2d tmpPoint = inPoints.at(i);posXs.push_back(tmpPoint.x);posYs.push_back(tmpPoint.y);}int k = degreeOfPolynomial; //多项式的次数//存储 sigma(xi),sigma(xi^2),sigma(xi^3)....sigma(xi^2n)std::vector<double> xValue(2 * k + 1);for (int i = 0; i < 2 * k + 1; i++){xValue[i] = 0;for (int j = 0; j < numPoints; j++) {xValue[i] = xValue[i] + pow(posXs[j], i);}}//Normal matrix(augmented)std::vector<std::vector<double>> matrixNormal(k + 1, std::vector<double>(k + 2, 0));//保存参数方程的系数std::vector<double> finalParas(k + 1);for (int i = 0; i <= k; i++) {for (int j = 0; j <= k; j++) {matrixNormal[i][j] = xValue[i + j];}}//存储sigma(yi)，sigma(xi*yi)，sigma(xi^2*yi)…sigma(xi^n*yi)std::vector<double> yValue(k + 1);for (int i = 0; i < k + 1; i++){yValue[i] = 0;for (int j = 0; j < numPoints; j++) {yValue[i] = yValue[i] + pow(posXs[j], i)*posYs[j];}}//加载yValue作为matrixNormal的最后一列（普通矩阵但增强）for (int i = 0; i <= k; i++) {matrixNormal[i][k + 1] = yValue[i];}//k变为n+1，因为下面的高斯消去部分是针对k个方程，但这里k是多项式的次数，对于k次我们得到k+1个方程k = k + 1;//高斯消元法求解线性方程组for (int i = 0; i < k; i++) {for (int n = i + 1; n < k; n++) {if (matrixNormal[i][i] < matrixNormal[n][i]) {for (int j = 0; j <= k; j++){double temp = matrixNormal[i][j];matrixNormal[i][j] = matrixNormal[n][j];matrixNormal[n][j] = temp;}}}}//循环执行高斯消除for (int i = 0; i < k - 1; i++){for (int n = i + 1; n < k; n++){double t = matrixNormal[n][i] / matrixNormal[i][i];for (int j = 0; j <= k; j++) {//使主元元素下面的元素等于0或消除变量matrixNormal[n][j] = matrixNormal[n][j] - t * matrixNormal[i][j];}}}//回代//使要计算的变量等于最后一个方程的rhs,然后减去除正在计算的变量的系数之外的所有lhs值,现在最后将 rhs 除以要计算的变量的系数for (int i = k - 1; i >= 0; i--){finalParas[i] = matrixNormal[i][k];for (int j = 0; j < k; j++) {if (j != i) {finalParas[i] = finalParas[i] - matrixNormal[i][j] * finalParas[j];}}finalParas[i] = finalParas[i] / matrixNormal[i][i];}QList<double> resParas;for (int i = 0; i < finalParas.size(); i++){qDebug() << "1--final:" << i << finalParas[i];resParas.push_back(finalParas[i]);}return resParas;
}//第二种方式--best
QList<double> polyfit(std::vector<cv::Point2d> &chain, int n)
{cv::Mat y(chain.size(), 1, CV_32F, cv::Scalar::all(0));/* ********【预声明phy超定矩阵】************************//* 多项式拟合的函数为多项幂函数* f(x)=a0+a1*x+a2*x^2+a3*x^3+......+an*x^n*a0、a1、a2......an是幂系数，也是拟合所求的未知量。设有m个抽样点，则：* 超定矩阵phy=1 x1 x1^2 ... ...  x1^n*           1 x2 x2^2 ... ...  x2^n*           1 x3 x3^2 ... ...  x3^n*              ... ... ... ...*              ... ... ... ...*           1 xm xm^2 ... ...  xm^n*  ΦT∗Φ∗a=ΦT∗y* *************************************************/cv::Mat phy(chain.size(), n, CV_32F, cv::Scalar::all(0));for (int i = 0; i < phy.rows; i++){float* pr = phy.ptr<float>(i);for (int j = 0; j < phy.cols; j++){pr[j] = pow(chain[i].x, j);}y.at<float>(i) = chain[i].y;}cv::Mat phy_t = phy.t();cv::Mat phyMULphy_t = phy.t()*phy;cv::Mat phyMphyInv = phyMULphy_t.inv();cv::Mat a = phyMphyInv * phy_t;a = a * y;qDebug() << a.rows << a.cols;std::cout << "res a = " << a << ";" << std::endl << std::endl;QList<double> resParas;for (int i = 0; i < a.rows; i++){qDebug() << "2--final:" << i << a.at<float>(i,0);resParas.push_back(a.at<float>(i, 0));}return resParas;
}//第三种方式  最小二乘曲线拟合
//x[n]       存放给定数据点的X坐标。
//y[n]       存放给定数据点的Y坐标。
//n          给定数据点的个数。
//a[m]       返回m - 1次拟合多项式的系数。
//m          拟合多项式的项数。要求m<=min(n,20)。
//dt[3]      分别返回误差平方和，误差绝对值之和与误差绝对值的最大值。
//void pir1(double x[], double y[], int n, double a[], int m, double dt[])
QList<double> least_squares_curve_fitting(std::vector<cv::Point2d> &inPoins, int m)
{double dt[3] = { 0.0 };int n = inPoins.size();std::vector<double> a(m);std::vector<double> x;std::vector<double> y;for (int i = 0; i < n; i++){cv::Point2d tmpPoint = inPoins.at(i);x.push_back(tmpPoint.x);y.push_back(tmpPoint.y);}int i, j, k;double p, c, g, q, d1, d2, s[20], t[20], b[20];for (i = 0; i <= m - 1; i++) a[i] = 0.0;if (m > n) m = n;if (m > 20) m = 20;b[0] = 1.0; d1 = 1.0*n; p = 0.0; c = 0.0;for (i = 0; i <= n - 1; i++){p = p + x[i]; c = c + y[i];}c = c / d1; p = p / d1;a[0] = c * b[0];if (m > 1){t[1] = 1.0; t[0] = -p;d2 = 0.0; c = 0.0; g = 0.0;for (i = 0; i <= n - 1; i++){q = x[i] - p; d2 = d2 + q * q;c = c + y[i] * q;g = g + x[i] * q*q;}c = c / d2; p = g / d2; q = d2 / d1;d1 = d2;a[1] = c * t[1]; a[0] = c * t[0] + a[0];}for (j = 2; j <= m - 1; j++){s[j] = t[j - 1];s[j - 1] = -p * t[j - 1] + t[j - 2];if (j >= 3)for (k = j - 2; k >= 1; k--)s[k] = -p * t[k] + t[k - 1] - q * b[k];s[0] = -p * t[0] - q * b[0];d2 = 0.0; c = 0.0; g = 0.0;for (i = 0; i <= n - 1; i++){q = s[j];for (k = j - 1; k >= 0; k--)  q = q * x[i] + s[k];d2 = d2 + q * q; c = c + y[i] * q;g = g + x[i] * q*q;}c = c / d2; p = g / d2; q = d2 / d1;d1 = d2;a[j] = c * s[j]; t[j] = s[j];for (k = j - 1; k >= 0; k--){a[k] = c * s[k] + a[k];b[k] = t[k]; t[k] = s[k];}}dt[0] = 0.0; dt[1] = 0.0; dt[2] = 0.0;for (i = 0; i <= n - 1; i++){q = a[m - 1];for (k = m - 2; k >= 0; k--) q = a[k] + q * x[i];p = q - y[i];if (fabs(p) > dt[2]) dt[2] = fabs(p);dt[0] = dt[0] + p * p;dt[1] = dt[1] + fabs(p);}QList<double> resParas;for (int i = 0; i < m; i++){qDebug() << "3--final:" << i << a[i];resParas.push_back(a[i]);}return resParas;
}// 测试程序
void curveFit() {std::vector<cv::Point2d> inPoints;inPoints.push_back(cv::Point2d(34, 36));inPoints.push_back(cv::Point2d(50, 82));inPoints.push_back(cv::Point2d(74, 142));inPoints.push_back(cv::Point2d(100, 200));inPoints.push_back(cv::Point2d(123, 242));inPoints.push_back(cv::Point2d(160, 281));inPoints.push_back(cv::Point2d(215, 236));inPoints.push_back(cv::Point2d(250, 150));inPoints.push_back(cv::Point2d(300, 84));inPoints.push_back(cv::Point2d(367, 74));inPoints.push_back(cv::Point2d(403, 139));inPoints.push_back(cv::Point2d(442, 550));inPoints.push_back(cv::Point2d(481, 300));QList<double> resParas3 = least_squares_curve_fitting(inPoints, 5);QList<double> resParas2 = polyfit(inPoints, 5);QList<double> resParas = discrete_point_fitting_curve(inPoints, 4);qDebug() << "resParas size:" << resParas;std::vector<cv::Point2d> newPoints;for (int i = 0; i < 500; i++){double newY = 0;for (int j = 0; j < resParas.size(); j++){newY += resParas.at(j) * pow(i, j);}newPoints.push_back(cv::Point(i, newY));newY = 0;}cv::Mat whiteImage = cv::Mat::zeros(500, 500, CV_8UC3);for (size_t i = 0; i < inPoints.size(); i++) {cv::circle(whiteImage, inPoints[i], 5.0, cv::Scalar(0, 0, 255), -1);}for (size_t i = 0; i < newPoints.size() - 1; i++) {cv::line(whiteImage, newPoints[i], newPoints[i + 1], cv::Scalar(0, 255, 255), 2, cv::LINE_AA);}//cv::imwrite("whiteImage.png", whiteImage);cv::namedWindow("curveFit");cv::imshow("curveFit", whiteImage);cv::waitKey(0);}

效果图:

k=3

k=4

k=5