利用Eigen库实现最小二乘拟合平面

卡死脱离态

已于 2023-08-21 10:11:22 修改

阅读量2.8k

点赞数 9

分类专栏：图形学相关文章标签：最小二乘法平面算法 c++

于 2022-07-12 12:07:43 首次发布

本文链接：https://blog.csdn.net/Akiyama_sou/article/details/125738908

版权

图形学相关专栏收录该内容

6 篇文章 3 订阅

订阅专栏

本文主要介绍最小二乘的直接求解法和SVD分解法，利用Eigen库进行矩阵运算求解。假设有一散点集合，当各点v到一平面S的距离d的平方和最小，该平面s即为最小二乘下的最优解。将距离平方和描述为平面拟合误差

$e=\sum_{i=1}^{n} d_{i}^{2}$ （1）

定义点类如下

class vec  
{
public:
	float v[3];
	vec(){}
	vec(float x,float y ,float z)
	{
		 v[0] = x; v[1] = y; v[2] = z;
	}
	const float &operator [] (int i) const
	{
		return v[i];
	}
	float &operator [] (int i)
	{
		return v[i];
	}
};

一、直接求解法

平面方程表示为 $A x+B y+C z+D=0(C \neq 0)$

两边同时除以C， $-\frac{A}{C} x-\frac{B}{C} \mathrm{y}-\frac{D}{C}=z$

令 $a_{0}=-\frac{A}{C}, a_{1}=-\frac{B}{C}, a_{2}=-\frac{D}{C}$

方程则可以表示为 $a_{0} x+a_{1} y+a_{2}=z$

先介绍下用定义求解的方式：

（1）式误差可以描述为 $e=\sum_{i=0}^{n-1}(a 0 * x+a 1 * y+a 2-z)^{2}$

要使e最小，应满足 $\frac{\partial e}{\partial a_{k}}=0, \quad \mathrm{k}=0,1,2$

列出如下方程

$\left\{\begin{array}{l} \sum 2\left(a_{0}x_{i}+a_{1} y_{i}+a_{2}-z_{i}\right) x_{i}=0 \\ \sum 2\left(a_{0} x_{i}+a_{1} y_{i}+a_{2}-z_{i}\right)y_{i}=0 \\ \sum 2\left(a_{0} x_{i}+a_{1} y_{i}+a_{2}-z_{i}\right)=0 \end{array}\right.$

改写为矩阵

$\left|\begin{array}{ccc} \Sigma \mathrm{x}_{\mathrm{i}}^{2} & \Sigma_{\mathrm{x}_{i} y_{i}} & \Sigma \mathrm{x}_{\mathrm{i}} \\ \Sigma \mathbf{x}_{\mathrm{i}} y_{i} & \Sigma \mathrm{y}_{\mathrm{i}}^{2} & \Sigma \mathrm{y}_{\mathrm{i}} \\ \Sigma_{\mathrm{x}_{\mathrm{i}}} & \Sigma_{\mathrm{y}_{\mathrm{i}}} & \mathrm{n} \end{array}\right|\left[\begin{array}{l} a_{0} \\ a_{1} \\ a_{2} \end{array}\right]=\left[\begin{array}{c} \Sigma_{\mathrm{x}_{i} z_{i}} \\ \Sigma \mathbf{y}_{\mathrm{i}} z_{i} \\ \Sigma_{\mathbf{z}_{\mathrm{i}}} \end{array}\right]$

求解上述矩阵获得a0,a1,a2的值，即可求得平面方程。代码如下

void ComputePlane(const vector<vec>&points, float &A, float &B, float &C, float &D)
{
	int n = points.size();
	MatrixXd mleft = MatrixXd::Zero(3, 3);//初始化左边项
	VectorXd b = VectorXd::Zero(3); //初始化右边项
	for (int i = 0; i < n; ++i)//遍历点装填两个矩阵
	{
		mleft(0, 0) += points[i][0] * points[i][0];//a11,xi平方和
		mleft(0, 1) += points[i][0] * points[i][1];//a12=a21,xiyi和
		mleft(0, 2) += points[i][0];//a13=a31,xi和
		mleft(1, 1) += points[i][1] * points[i][1];//a22,yi平方和
		mleft(1, 2) += points[i][1];//a23=a32,yi和
		b[0] += points[i][0] * points[i][2];//xizi和
		b[1] += points[i][1] * points[i][2];//yizi和
		b[2] += points[i][2];//zi和
	}
	mleft(2, 2) = n;//a33,n
	mleft(1, 0) = mleft(0, 1);
	mleft(2, 0) = mleft(0, 2);
	mleft(2, 1) = mleft(1, 2);
	VectorXd x = mleft.fullPivHouseholderQr().solve(b);//QR分解求解
	float a0 = x[0]; float a1 = x[1]; float a2 = x[2];//为了便于理解，赋值给临时变量
	A = a0; B = a1; C = -1; D = a2;
}

然而这种方式还需要计算左边项和右边项各元素的值，相对繁琐，Eigen库可以直接求解超定方程组，左边项为散点x，y值与1，右边项为z值，列出如下方程组直接求解得到的即是最小二乘解。

$\left[\begin{array}{ccc} x_{1} & y_{1} & 1 \\ x_{2} & y_{2} & 1 \\ \vdots & \vdots & \vdots \\ x_{n} & y_{n} & 1 \end{array}\right]\left[\begin{array}{l} a_{0} \\ a_{1} \\ a_{2} \end{array}\right]=\left[\begin{array}{c} z_{1} \\ z_{2} \\ \vdots \\ z_{n} \end{array}\right]$

实现代码如下，传入点集points和待求解平面系数ABCD。不论是采用定义法建立3*3的左边项还是解如下超定方程组，获得的结果是一样的，证明Eigen库求解超静定问题内部采用的是最小二乘的思想。

void ComputePlane(const vector<vec>&points, float &A, float &B, float &C, float &D)
{
	int n = points.size();
	MatrixXd mleft = MatrixXd::Ones(n, 3);//初始化左边项
	VectorXd b = VectorXd::Zero(n); //初始化右边项
	for (int i = 0; i < n; ++i)//遍历点装填两个矩阵
	{
		mleft(i, 0) = points[i][0];
		mleft(i, 1) = points[i][1];
		b[i] = points[i][2];
	}
	VectorXd x = mleft.fullPivHouseholderQr().solve(b);//QR分解求解
	float a0 = x[0]; float a1 = x[1]; float a2 = x[2];//为了便于理解，赋值给临时变量
	A = a0; B = a1; C = -1; D = a2;
}

二、SVD分解法

平面方程表示为ax+by+cz = d，（1）

约束条件为 $a^{2}+b^{2}+c^{2}=1$

计算散点坐标的平均值，应满足 $a \bar{x}+b \bar{y}+c \bar{z}=d$ （2）

（1）式代入各个散点，（1）-（2）得（3）

列为矩阵，式（3）等价于AX = 0

$A=\left[\begin{array}{ccc} x_{1}-\bar{x} & y_{1}-\bar{y} & z_{1}-\bar{z} \\ x_{2}-\bar{x} & y_{2}-\bar{y} & z_{2}-\bar{z} \\ \vdots & \vdots & \vdots \\ x_{n}-\bar{x} & y_{n}-\bar{y} & z_{n}-\bar{z} \end{array}\right], X=\left[\begin{array}{l} a \\ b \\ c \end{array}\right]$

目标函数为 $\min \|A X\|$ ，约束条件 $\|X\|=1$ ，对做奇异值分解 $A=U D V^{T}$ ，则 $\|A X\|=\left\|U D V^{T} X\right\|=\left\|D V^{T} X\right\|$ ，且 $\left\|V^{T} X\right\|=\|X\|=1$ ，假设D的最后一个对角元素为最小奇异值，当且仅当

$V^{T} X=\left[\begin{array}{c}0 \\ 0 \\ \vdots \\ 1\end{array}\right]$

时，取得 $\min \|A X\|$ ，此时最小奇异值对应的特征向量即是平面的法向量，即a,b,c。

在Eigen库中，有现成的JacobiSVD类供调用，代码实现如下

void ComputePlane(const vector<vec>&points, float &A, float &B, float &C, float &D)
{
	int n = points.size();
	MatrixXd mleft = MatrixXd::Zero(n, 3);//初始化矩阵A
	vec averagepoint(0, 0, 0);//平均坐标
	for (int i = 0; i < n; ++i)
	{
		averagepoint[0] += points[i][0] / n;
		averagepoint[1] += points[i][1] / n;
		averagepoint[2] += points[i][2] / n;
	}
	for (int i = 0; i < n; ++i)//遍历点装填矩阵
	{
		mleft(i, 0) = points[i][0] - averagepoint[0];
		mleft(i, 1) = points[i][1] - averagepoint[1];
		mleft(i, 2) = points[i][2] - averagepoint[2];
	}
	JacobiSVD<MatrixXd> svd(mleft, ComputeFullU | ComputeFullV);//对矩阵A分解
	MatrixXd vt = svd.matrixV().transpose();//获得矩阵v的转置
	Vector3d b(0, 0, 1);
	VectorXd x = vt.fullPivHouseholderQr().solve(b);
	A = x[0]; B = x[1]; C = x[2];
	D = A*averagepoint[0] + B*averagepoint[1] + C * averagepoint[2];
}

main函数的内容和输出结果如下。输入随机点个数n，随机点的坐标值为0-10的浮点型，输出点坐标集和最后拟合的平面方程。

int main()
{
	vector<vec>points;
	int n = 0;
	cin >> n;
	for (int i = 0; i < n;i++)
	{
		float x, y, z;//在0-1内取随机数
		x = double(rand()) / RAND_MAX*10;
		y = double(rand()) / RAND_MAX*10;
		z = double(rand()) / RAND_MAX*10;
		points.push_back(vec(x,y,z));
	}
	for (int i = 0; i < n; i++)
	{
		cout << "(" << points[i][0] << "," << points[i][1] << "," << points[i][2] << ")" << endl;
	}
	float A, B, C, D;
	ComputePlane(points, A, B, C, D);
	cout << "平面方程为 "<<A << "x+" << B << "y+" << C << "z =" << D << endl;
	system("pause");
	return 0;
}

在做自己的项目过程中，发现svd分解法得到的平面更加准确，如下柱状图拟合1000个平面，横坐标表示平面的序号，纵坐标表示拟合点距离该平面的最大距离，左边是svd分解的结果，最大值在1.2以下，而右边直接求解的结果大于1.2的平面很多，当然检查它们的距离平方和结果也是一样的，svd同样优于直接求解，感兴趣的可以自行验证下。