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

本文主要介绍最小二乘的直接求解法和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(n); //初始化右边项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,nmleft(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);//初始化矩阵Avec 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同样优于直接求解，感兴趣的可以自行验证下。

参考链接

最小二乘拟合平面(C++版) - 知乎