3D點雲的最小二乘法求解-數學推導 | LKY 只有原創內容的 Blog

平面方程式

如果有一群 3D 的點，要如何 regression 一個平面出來？

平面方程式：

1	Ax + By + Cz + D = 0

但是把 Cz 移動到等號另一邊，C 就可以消掉，所以實際上只有三個未知數：

1	(A/C)x + (B/C)y + (D/C) = z

C 可以是除了0或者無限大以外的任何實數，因此，在這裡就不重要了。

為了簡化，重寫平面方程式：

1	Ax + By + C = z

以下就用 Ax + By + C = z 推導

無誤差情況：Ax + By + C = z
實際上情況：Ax + By + C = f(x, y)
也就是說，存在誤差：z - f(x, y)

為了同時消除方向性、逞罰大誤差，所以將誤差平方：( z - f(x, y) )^2
將誤差方程式寫出來，求出微分=0的極值點，就能得到此方程式參數。

1
2
3

Err
= ( z - f(x, y) )^2
= ( z - Ax - By - C )^2

有多個未知數 A, B, C，因此需要分別對 A, B, C 微分：

1
2
3

dErr/dA = ( z - Ax - By - C )(-x)
dErr/dB = ( z - Ax - By - C )(-y)
dErr/dC = ( z - Ax - By - C )(-1)

（ x, y, z 都是已有的點雲數據集，都有值！所以未知數是 A, B, C！不是 x, y, z！別搞錯了！）

（我自己寫的時候都搞錯了！寫成「對 x, y, z 微分」，囧）

根據我們的需要與期望，要找出誤差變化率為0的點。這一點的誤差，不是最大就是最小。

1
2
3

0 = ( z - Ax - By - C )(-x)
0 = ( z - Ax - By - C )(-y)
0 = ( z - Ax - By - C )(-1)

把 chain rule 長出來的這一項乘進去：

1
2
3

0 = -xz + Ax^2 + Bxy + Cx
0 = -yz + Axy + By^2 + Cy
0 =   -z + Ax + By + C

把已知項移到等號左邊（再次提醒！未知數是 A, B, C）：

1
2
3

xz = Ax^2 + Bxy + Cx
yz = Axy + By^2 + Cy
 z = Ax + By + C

第一條，就是原本的平面方程式同乘x
第二條，就是原本的平面方程式同乘y
第三條，剛好就是原本的平面方程式

寫成矩陣形式：

Ax = b
b = [xz, yz, z]
x = [A, B, C]
A = [
 [x^2 + xy + 1],
 [xy + y^2 + 1],
 [x + y + 1]
]

矩陣計算沒有除法，只能用反矩陣搬移。兩邊同時左乘A的逆矩陣：

1
2
3

(A^-1)Ax = (A^-1)b
(A^-1)A = I(單位矩陣)
x = (A^-1)b

由此解得所有未知數。

逆矩陣計算也是超複雜的，我忘光了，工程數學課本都有，在此不贅述。

以調包的角度來看，到這裡已經可以輕易使用現成工具實現了。

應該沒有哪種程式語言，找不到現成逆矩陣算法的（雖然有的語言比較難找，比如 C#，官方有 System.Numerics.Matrix4x4.Invert，但知名度還遠不如需要另外安裝的第三方套件來得高）。

下一篇文章會說如何用 Python 底層實現這個演算法，不使用np.linalg.lstsq、scipy.optimize.leastsq這些已經寫好的方法。