文章目录
- 使用离散点拟合曲线
- 参考代码路径:
- 作者源码:
- 测试代码
- 效果图:
- k=3
- k=4
- k=5
使用离散点拟合曲线
参考代码路径:
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
