通过离散点拟合曲线

使用离散点拟合曲线

参考代码路径:

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 precision
    cout.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 values
    cin>>N;
    double x[N],y[N];
    cout<<"\nEnter the x-axis values:\n";                //Input x-values
    for (i=0;i<N;i++)
        cin>>x[i];
    cout<<"\nEnter the y-axis values:\n";                //Input y-values
    for (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 coefficients
    for (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 matrix
    double 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 equations
    cout<<"\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 elimination
        for (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 equation
        for (j=0;j<n;j++)
            if (j!=i)            //then subtract all the lhs values except the coefficient of the variable whose value                                   is being calculated
                a[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