最小二乘法拟合多项式曲线（含C++代码）

本文原创，转载请注明来源：https://blog.csdn.net/syc666946/article/details/79954587

最小二乘原理见https://blog.csdn.net/jairuschan/article/details/7517773

关于原文下面评论中的问题：为什么最后一步明明是求解线性方程组AX=Y(X， Y均为向量)中的系数矩阵A，

而A的表达式为A = (X'*X)-1*X'*Y，这是一个超定方程组，A矩阵是奇异矩阵，所以A并不可直接求逆，只能求伪逆。

C++代码如下：https://github.com/shaoyuncen/MyPCL/blob/master/LeastSquare/LeastSquare/class.h

矩阵计算部分用了Eigen库，其中的输入vector中的数据类型MyPoint是我自己定义的X,Y,Z点类型，具体见github。

compute方法输入为同一平面上的点，用于估计系数矩阵A； 具体拟合到几次方可以设置_POWER

#include <iostream>
#include <vector>
#include <math.h>
#include <fstream>
#include <sstream>
#include "Eigen/Eigen"
#include <Eigen/SVD>
int _POWER; //参数个数， _POWER-1等于拟合的最高次数

class LeastSquare {
private:
	Eigen::RowVectorXd coeficients;//系数矩阵
	double Deviation;
	double k, b;

public:
	LeastSquare(const vector<MyPoint> &ptr) {//ptr是一条线的点云数据
		coeficients.setZero();//初始化为0矩阵
		compute(ptr);//计算估计参数矩阵，拟合曲线；
		//compute1(ptr);
	}
	double getk() { return k; }
	double getb() { return b; }
	double power(const double number, int power) 
	{
		double temp = 1;
		while (power--)
		{
			temp *= number;
		}
		return temp;
	}
	void compute1(const vector<MyPoint> &ptr);//compute the k and b;
	void compute(const vector<MyPoint> &ptr);parameter estimate---compute coeficients
	void filter(const vector<MyPoint> &ptr, vector<MyPoint> &outlier);
	void fucking_print()
	{
		//cout << "y = " << k << "x + " << b << "\t";
		cout << "曲线的多项式系数（形如y = a0 + a1*x + a2*x2 +...am*xm）为:\n" << coeficients << endl;
	}
};

void LeastSquare::compute1(const vector<MyPoint> &ptr) //compute k and b， 这个是之前写的拟合直线的，下面的多项式拟合次数取2和这个方法结果一致
{
	double SumXi = 0.0;
	double SumYi = 0.0;
	double SumXi_2 = 0.0;
	double SumXiYi = 0.0;
	double deviation = 0.0;
	int number = int(ptr.size());//size(返回的是无符号整型，需要强制类型转换后使用)
	for (int i = 0; i < number;i++)
	{
		SumXi += ptr[i].x;
		SumYi += ptr[i].z;
		SumXi_2 += ptr[i].x * ptr[i].x;
		SumXiYi += ptr[i].x * ptr[i].z;
	}

	double temp = (number * SumXi_2 - SumXi * SumXi);//减少计算次数
	k = (number * SumXiYi - SumXi * SumYi) / temp;
	b = (SumXi_2 * SumYi - SumXi * SumXiYi) / temp;
}
void LeastSquare::compute(const vector<MyPoint> &ptr) //多项式拟合的参数估计
{
	if (_POWER < 2) return; //参数至少两个
	const int PointNum = int(ptr.size());
	Eigen::RowVectorXd Xi_1(PointNum);//Xi的一次矩阵，1行n列
	Eigen::RowVectorXd Yi_1(PointNum);//Yi的一次矩阵，1行n列
	Eigen::MatrixXd A(PointNum, _POWER);//Ax = b中的A，结构矩阵，x为系数矩阵coeficients，b为数据矩阵，即Yi向量（1 * n的矩阵）,power为次数
	Xi_1.setZero();
	Yi_1.setZero();
	A.setOnes();
	for (unsigned int i = 0; i < ptr.size(); i++)//Ps：行的下标对应文件的行数-2，因为读文件的时候第一行会被忽略，然后下标又是从0开始
	{
		Xi_1[i] = ptr[i].x;
		Yi_1[i] = ptr[i].z;
		for (int cols = 0; cols < _POWER; cols++)
		{
			A(i, cols) = power(ptr[i].x, cols);
		}
		//A(i, _POWER - 1) = ptr[i].x;
	}
	Eigen::MatrixXd temp;
	temp = A.transpose()* A;
	coeficients = (temp.inverse() * A.transpose() ) * Yi_1.transpose();
	
	//coeficients = A.ldlt().solve(Yi_1); // A sym. p.s.d.    #include <Eigen/Cholesky>
	//coeficients = A.llt().solve(Yi_1);  // A sym. p.d.      #include <Eigen/Cholesky>
	//coeficients = A.lu().solve(Yi_1);  // Stable and fast. #include <Eigen/LU>
	//coeficients = A.qr().solve(Yi_1);  // No pivoting.     #include <Eigen/QR>
	//coeficients = A.svd().solve(Yi_1);  // Stable, slowest. #include <Eigen/SVD>
	
}