最小二乘法——拟合平面方程（深度相机外参标定、地面标定）

1.最小二乘法

最小二乘法（又称最小平方法）是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。最小二乘法还可用于曲线拟合。其他一些优化问题也可通过最小化能量或最大化熵用最小二乘法来表达。

最小二乘法的矩阵形式为：
$Ax=b$
其中 $A$ 为 $n * k$ 的矩阵，$x$ 为 $k*1$ 的列向量，$b$ 为 $n*1$ 的列向量。如果 $n>k$（方程的个数大于未知量的个数），这个方程系统称为矛盾方程组 Over Determined System，如果 $n<k$（方程的个数小于未知量的个数），这个系统就是Under Determined System。

当找到向量 $x$ 使得 $||Ax-b||$ 最小，则 $x$ 为该方程的最小二乘解。

求解最小二乘的方法有奇异值分解、正规方程、QR分解三种。本文中采用正规方程对平面方程进行拟合，以实现深度相机的外参标定。正规方程组的解为：
$x=(A^TA)^{-1}A^Tb$

2.平面方程拟合

平面方程的一般表达式为
$Ax+By+Cz+D=0 （C\neq0）$
将其变换为如下形式
$z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}$
令$a_0=-\frac{A}{C};$ $a_1=-\frac{B}{C};$ $a_2=-\frac{D}{C};$
$z=a_0x+a_1y+a_2$
此时对应的最小二乘矩阵形式

$A=\begin{pmatrix} x_1&y_1&1\\x_2&y_2&1\\...\\x_n&y_n&1\end{pmatrix}; x=\begin{pmatrix}a_0\\a_1\\a_2\end{pmatrix};b=\begin{pmatrix} z_1 \\ z_2 \\ ...\\ z_n\end{pmatrix};(n\geq3)$

其中$(x_1,y_1,z_1),(x_2,y_2,z_2),...,(x_n,y_n,z_n)$为输入的三维点座标。

套用正规方程组的解，即可求得$(a_0,a_1,a_2);$

3.标定——构造旋转矩阵

在实际使用中，经常会采用地面作为参照平面，将相机座标系转化为世界座标系，本文中使用最小二乘法对地面点云拟合平面方程，将相机座标系Z轴旋转至垂直地面。

如上求解出地面的平面方程系数，则平面方程一般式为：
$a_0x+a_1y-z+a_2=0$
其法向量为：
$(\frac{a_0}{\sqrt{a_0^2+a_1^2+1}},\frac{a_1}{\sqrt{a_0^2+a_1^2+1}},\frac{-1}{\sqrt{a_0^2+a_1^2+1}})$
求得平面法向量的单位向量为 $\vec{n}$。
相机座标系的Z轴向量 $\vec{z}$ 为$(0,0,1)$。
旋转向量为 $\vec{r}$，其中 $\vec{r}$ 方向为 $\vec{n}\times\vec{z}$ ，旋转角度为 $\theta=arccos(\vec{n}\cdot\vec{r})$

使用 Eigen::AngleAxisd 将旋转向量转化为 Eigen::Matrix3d 的旋转矩阵。

4.代码

	//0.最小二乘拟合平面方程
	//planePoints存储相机座标系选择地面区域内的所有三维点云
	Eigen::MatrixXd A(planePoints.size(), 3);
	Eigen::VectorXd b(planePoints.size());
	//将观测点输入矩阵
	for (int i = 0; i < planePoints.size(); i++)
	{
		A(i, 0) = planePoints[i].x;
		A(i, 1) = planePoints[i].y;
		A(i, 2) = 1;
		b(i) = planePoints[i].z;
	}
	Eigen::MatrixXd AT = A.transpose();
	//使用最小二乘法求得系数向量
	Eigen::Vector3d x = (AT*A).inverse()*AT*b;

	//1.求解旋转矩阵
	//单位法向量
	double denominator = sqrt(x(0)*x(0) + x(1)*x(1) + 1);
	Eigen::Vector3d n(x(0) / denominator, x(1) / denominator, -1 / denominator);
	n = n.normalized();
	Eigen::Vector3d zdir(0, 0, 1);
	//求解两向量的旋转向量，点乘求夹角、叉乘求旋转方向。
	Eigen::AngleAxisd rotateVector(acos(n.dot(zdir)), n.cross(zdir).normalized());
	//获取旋转矩阵
	Eigen::Matrix3d zRotateMatrix = rotateVector.matrix();

5.结果

首先选择一系列三维点云（蓝色代表有点云），如下图：

未转化的相机座标系下三维点云如下图：

用上述构造旋转矩阵进行点云座标系变换，令Z轴方向垂直地面，结果如下：