最小二乘法——擬合平面方程（深度相機外參標定、地面標定）

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軸方向垂直地面，結果如下：