最小二乘曲线拟合的C++实现

Posted 2021-08-05 借我十斤肉

文章目录

• • 1 前言
  • 2 数据说明
  • 3 代码实现
  • 4 测试数据下载

---

1 前言

在之前的博客已经讲过了 曲线拟合的最小二乘原理 以及 如何求解矩阵方程，下面学以致用，采用C++实现最小二乘曲线拟合/最小二乘多项式拟合。

需要明确的一点是，对于同一组数据，以X为自变量和以Y为自变量进行拟合的误差会有所差别。

2 数据说明

本次采用的数据为pcd点云文件，是一条离散的曲线点。因此需要事先 配置pcl库，比较方便的是，pcl库包含了Eigen库，因此不必单独配置Eigen

待拟合数据点

3 代码实现

代码包括3部分

• main.cpp：最小二乘曲线拟合实现，需要依次设置输入点云、拟合阶数、是否交换XY、最后执行拟合以及均方根误差的计算。
• leastSquareCurveFitting.h：最小二乘曲线拟合相关函数声明
• leastSquareCurveFitting.cpp：最小二乘曲线拟合相关函数定义

main.cpp

/*
	=============最小二乘曲线拟合==============

	需要设置的参数：
	* 输入点云
	* 拟合次数
	* 是否交换XY
	* 执行拟合
	* 误差计算
*/

#include "leastSquareCurveFitting.h"

int main()
{
	//-------------------------------
	//----------加载曲线点云----------
	//-------------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile("test_date.pcd", *cloud_in) < 0)
	{
		PCL_ERROR("点云文件不存在！\\a\\n");
	}
	cout << "->加载点的个数：" << cloud_in->size() << endl;

	//----------------------------------
	//----------最小二乘曲线拟合----------
	//----------------------------------
	LeastSquareCurveFitting lscf;	//创建最小二乘曲线拟合对象
	lscf.setInputCloud(cloud_in);	//设置输入点云
	lscf.setFitTimes(2);			//设置拟合阶数
	lscf.swapXY(true);				//是否交换XY
	lscf.leastSquareCurveFitting();	//执行最小二乘曲线拟合
	lscf.calculateRMSE();			//计算拟合均方根误差

	return 0;
}

leastSquareCurveFitting.h

#pragma once
#include <iostream>
#include <pcl/io/pcd_io.h>

using namespace std;
using namespace Eigen;

//最小二乘曲线拟合类
class LeastSquareCurveFitting
{
public:

	/**
	* @brief   : 设置输入点云
	* @param[I]: cloud_in (输入点云)
	* @param[O]: none
	* @return  : none
	* @note    :
	**/
	void setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);

	/**
	* @brief   : 设置拟合阶数
	* @param[I]: poly_m (拟合阶数)
	* @param[O]: none
	* @return  : none
	* @note    :
	**/
	void setFitTimes(int poly_m = 1);
	
	/**
	* @brief   : 交换X、Y,以Y为自变量，X为因变量拟合
	* @param[I]: is_swap (是否交换，默认false，不交换)
	* @param[O]: none
	* @return  : none
	* @note    :
	**/
	void swapXY(bool is_swap = false);

	/**
	* @brief   : 最小二乘曲线拟合
	* @param[I]: none
	* @param[O]: none
	* @return  : none
	* @note    :
	**/
	void leastSquareCurveFitting();

	/**
	* @brief   : 拟合均方根误差计算
	* @param[I]: none
	* @param[O]: none
	* @return  : none
	* @note    :
	**/
	void calculateRMSE();


private:

	pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_in;		//输入点云
	bool is_setInputCloud = false;						//是否设置输入点云
	int poly_m;											//拟合阶数
	bool is_swap = false;										//是否交换XY
	vector<float> m_a;									//拟合系数，用于拟合误差计算

};

leastSquareCurveFitting.cpp

#include "leastSquareCurveFitting.h"

/**
* @brief   : 设置输入点云
* @param[I]: cloud_in (输入点云)
* @param[O]: none
* @return  : none
* @note    :
**/
void LeastSquareCurveFitting::setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
	m_cloud_in = cloud_in;
	is_setInputCloud = true;
}

/**
* @brief   : 设置拟合阶数
* @param[I]: poly_m (拟合阶数)
* @param[O]: none
* @return  : none
* @note    :
**/
void LeastSquareCurveFitting::setFitTimes(int poly_m)
{
	this->poly_m = poly_m;
}

/**
* @brief   : 交换X、Y,以Y为自变量，X为因变量拟合
* @param[I]: is_swap (是否交换，默认false，不交换)
* @param[O]: none
* @return  : none
* @note    :
**/
void LeastSquareCurveFitting::swapXY(bool is_swap)
{
	this->is_swap = is_swap;
}

/**
	* @brief   : 最小二乘曲线拟合
	* @param[I]: none
	* @param[O]: none
	* @return  : none
	* @note    :
**/
void LeastSquareCurveFitting::leastSquareCurveFitting()
{
	if (!is_setInputCloud)
	{
		PCL_ERROR("->请设置输入点云！\\a\\n");
		system("pause");
		abort();
	}

	//将坐标放入vector容器
	vector<double> vec_x;
	vector<double> vec_y;
	if (is_swap)
	{
		for (size_t i = 0; i < m_cloud_in->size(); i++)
		{
			vec_x.push_back(m_cloud_in->points[i].y);
			vec_y.push_back(m_cloud_in->points[i].x);
		}
	}
	else
	{
		for (size_t i = 0; i < m_cloud_in->size(); i++)
		{
			vec_x.push_back(m_cloud_in->points[i].x);
			vec_y.push_back(m_cloud_in->points[i].y);
		}
	}
	

	//矩阵方程 Ga=d
	MatrixXd G(poly_m + 1, poly_m + 1);//系数矩阵G
	MatrixXd d(poly_m + 1, 1);//常数项矩阵d

	//计算系数矩阵G
	double sum_x;//x的k次幂的累加和，k=0,1,...,n
	for (int row = 0; row < poly_m + 1; row++)
	{
		for (int col = 0; col < poly_m + 1; col++)
		{
			sum_x = 0.0;
			//计算x的（row+col)次幂的和
			for (int i = 0; i < vec_x.size(); i++)
			{
				sum_x += pow(vec_x[i], (row + col));
			}
			G(row, col) = sum_x;	//系数矩阵元素赋值
		}
	}
	cout << "\\n方程组系数矩阵：\\n" << G << endl;

	//计算常数矩阵d
	double sum_d;	//常数矩阵元素
	for (int row = 0; row < poly_m + 1; row++)
	{
		sum_d = 0.0;
		//计算x的（row+col)次幂的和
		for (int i = 0; i < vec_x.size(); i++)
		{
			sum_d += vec_y[i] * (pow(vec_x[i], row));
		}
		d(row, 0) = sum_d;//系数矩阵元素赋值
	}
	cout << "方程组常数数矩阵：\\n" << d << endl;

	//===================================================法方程 Ga=d 求解的多种方式，选其一即可==============================↓

	//----------householderQr()分解----------
	MatrixXd HQR(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，householderQr分解求解矩阵方程
	HQR = G.householderQr().solve(d);
	cout << "\\n->householderQr分解得到的拟合系数矩阵为：\\n" << HQR << endl;

	//----------colPivHouseholderQr分解----------
	MatrixXd CPHQR(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，colPivHouseholderQr分解求解矩阵方程
	CPHQR = G.colPivHouseholderQr().solve(d);
	cout << "\\n->colPivHouseholderQr分解得到的拟合系数矩阵为：\\n" << CPHQR << endl;

	//----------fullPivHouseholderQr分解----------
	MatrixXd FPHQR(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，fullPivHouseholderQr分解求解矩阵方程
	FPHQR = G.fullPivHouseholderQr().solve(d);
	cout << "\\n->fullPivHouseholderQr分解得到的拟合系数矩阵为：\\n" << FPHQR << endl;


	//----------LLT分解----------
	MatrixXd LLT(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，LLT分解求解矩阵方程
	LLT = G.llt().solve(d);
	cout << "\\n->Cholesky分解得到的拟合系数矩阵为：\\n" << LLT << endl;

	//----------LDLT分解----------
	MatrixXd LDLT(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，LDLT分解求解矩阵方程
	LDLT = G.ldlt().solve(d);
	cout << "\\n->LDLT分解得到的拟合系数矩阵为：\\n" << LDLT << endl;

	//----------BDSVD分解----------
	MatrixXd BDCSVD(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，BDCSVD分解求解矩阵方程
	BDCSVD = G.bdcSvd(ComputeThinU | ComputeThinV).solve(d);
	cout << "\\n->BDSVD分解得到的拟合系数矩阵为：\\n" << BDCSVD << endl;

	//----------JacobiSVD分解----------
	MatrixXd JacobiSVD(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，JacobiSVD分解求解矩阵方程
	JacobiSVD = G.jacobiSvd(ComputeThinU | ComputeThinV).solve(d);
	cout << "\\n->jacobiSvd分解得到的拟合系数矩阵为：\\n" << JacobiSVD << endl;


	//----------LU分解----------
	MatrixXd LU(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，LU分解求解矩阵方程
	LU = G.lu().solve(d);
	cout << "\\n->LU分解得到的拟合系数矩阵为：\\n" << LU << endl;

	//----------fullPivLu分解----------	
	//MatrixXd FPLU(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，FullPivLU分解求解矩阵方程
	//FPLU = G.fullPivLu().solve(d);	//error C1128: 节数超过对象文件格式限制: 请使用 /bigobj 进行编译
	//cout << "\\n->fullPivLu分解得到的拟合系数矩阵为：\\n" << FPLU << endl;


	//----------partialPivLu分解----------
	MatrixXd PPLU(poly_m + 1, 1);//待求多项式系数矩阵[a0,a1,...,an]'，PartialPivLU分解求解矩阵方程
	PPLU = G.partialPivLu().solve(d);
	cout << "\\n->partialPivLu分解得到的拟合系数矩阵为：\\n" << PPLU << endl;

	//----------直接求逆----------
	if (G.determinant() > 0)
	{
		//矩阵行列式大于0，
		cout << "\\n->直接求逆法的拟合系数矩阵为：\\n" << G.inverse()*d << endl;
	}

	//===================================================矩阵方程求解的多种方式，选其一即可==============================↑

	//-------------------------------
	//----------输出拟合方程----------
	//-------------------------------
	if (is_swap)
	{
		cout << "\\n->" << poly_m << "阶拟合方程为：" << "x = " << CPHQR(0, 0) << " + " << CPHQR(1, 0) << "y";
		for (int m = 2; m < poly_m + 1; m++)
		{
			cout << " + " << CPHQR(m, 0) << "y^" << m;
		}
		cout << endl;
	}
	else
	{
		cout << "\\n->" << poly_m << "阶拟合方程为：" << "y = " << CPHQR(0, 0) << " + " << CPHQR(1, 0) << "x";
		for (int m = 2; m < poly_m + 1; m++)
		{
			cout << " + " << CPHQR(m, 0) << "x^" << m;
		}
		cout << endl;
	}
	

	//提取拟合系数，用于拟合误差计算
	for (int i = 0; i < poly_m + 1; i++)
	{
		m_a.push_back(CPHQR(i, 0));
	}

}

/**
* @brief   : 拟合均方根误差计算
* @param[I]: none
* @param[O]: none
* @return  : none
* @note    :
**/
void LeastSquareCurveFitting::calculateRMSE()
{
	float rss = 0.0f;		//剩余平方和RSS(residual sum of square)即残差平方和,是实际值与估计值之差的平方的总和,也就是误差项平方的总和。
	float rmse = 0.0f;	//均方根误差

	float yi;//拟合后因变量y的值
	if (is_swap)
	{
		for (size_t i = 0; i < m_cloud_in->size(); i++)
		{
			yi = 0.0f;
			for (size_t j = 0; j < m_a.size(); j++)
			{
				yi += m_a[j] * pow(m_cloud_in->points[i].y, j);
			}

			rss += pow((yi - m_cloud_in->points[i].x), 2);
		}
	}
	else
	{
		for (size_t i = 0; i < m_cloud_in->size(); i++)
		{
			yi = 0.0f;
			for (size_t j = 0; j < m_a.size(); j++)
			{
				yi += m_a[j] * pow(m_cloud_in->points[i].x, j);
			}

			rss += pow((yi - m_cloud_in->points[i].y), 2);
		}
	}
	
	rmse = sqrt(rss / m_cloud_in->size());

	cout << "\\n->拟合均方根误差：" << rmse << endl;
}

输出结果：

->加载点的个数：155

方程组系数矩阵：
         155       -26665  4.93662e+06
      -26665  4.93662e+06 -9.69448e+08
 4.93662e+06 -9.69448e+08   1.9922e+11
方程组常数数矩阵：
   -4397.09
     832518
-1.6597e+08

->householderQr分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->colPivHouseholderQr分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->fullPivHouseholderQr分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->Cholesky分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->LDLT分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->BDSVD分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->jacobiSvd分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->LU分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->partialPivLu分解得到的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->直接求逆法的拟合系数矩阵为：
    19.4176
   0.347728
0.000377855

->2阶拟合方程为：x = 19.4176 + 0.347728y + 0.000377855y^2

->拟合均方根误差：0.0147226

通过对比不同矩阵方程求解方式的结果，可以看出不管哪种方式，得到的系数矩阵都相同。

实际应用中，选择其中一种方式即可。

为适用于不同的数据，选用求解精度较高的 colPivHouseholderQr分解得到的结果作为拟合系数。

以不同分量为自变量的拟合误差

自变量	拟合均方根误差
X	0.519974
Y	0.0147226

4 测试数据下载

test_date.pcd

提取码：pdx6