用 SVD 解 ICP 問題，找出點雲之間的幾何變換關係

本篇文章要介紹的是一種找到兩個點雲之間的平移旋轉關係的傳統方法。前面我們介紹了兩種深度學習的方法：PointNetLK 與 PCRNet，而這篇介紹的方法是用 SVD 來解出兩個點雲之間的旋轉與平移關係式。

問題定義

給定兩個點雲 \(P\) 及 \(Q\)，找到一組旋轉矩陣 \(R\) 及平移向量 \(t\) 使得利用 \(R\)，\(t\) 將 \(P\) 變換以後與 \(Q\) 的誤差最小，也就是：
\[ (R,t) = arg\ min\ \sum_{i=1}^{n} w_i \left \| (R p_i + t) - q_i \right \|^2 \] 其中 \(w_i\) 代表每個點的權重。 

解出平移向量 \(t\)

把以上式子對 \(t\) 的偏微分為零可得：
\[ 0 = \frac{\partial F}{\partial t}= \sum_{i=1}^{n}2w_i(Rp_i+t-q_i) \\ = 2t\ (\sum w_i)+ 2R\ (\sum w_i p_i)-2\ (\sum w_i q_i) \] 因此在找出 \(R\) 的情況下可以解出 \(t\)： \[ t = \overline{q} - R \overline{p} \\ \overline{q}= \frac{\sum w_i q_i}{\sum w_i} ,\ \overline{p}= \frac{\sum w_i p_i}{\sum w_i} \] 也就是說 \(\overline{p}, \overline{q}\) 為點雲 \(P\) 與 \(Q\) 的質心。 

我們將算好的 \(t\) 代回原式子： \[ \sum_{i=1}^{n} w_i \left \| (R p_i + t) - q_i \right \|^2 = \sum_{i=1}^{n} w_i \left \| R p_i + \overline{q} - R \overline{p} - q_i \right \|^2 \\ = \sum_{i=1}^{n} w_i \left \| R (p_i - \overline{p}) - (q_i - \overline{q})\right \|^2 \] 

因此如果將 \(P\) 與 \(Q\) 之中的每個點減去他們各自的質心得到 \(x_i, y_i\)： \[ x_i = p_i - \overline{p}, y_i = q_i - \overline{q} \]則求解 \(R\) 的式子則簡化為： \[ R = arg\ min\ \sum_{i=1}^{n} w_i \left \| R x_i - y_i \right \|^2 \] 

解出旋轉矩陣 \(R\)

首先來整理一下上面的式子： \[ \left \| R x_i - y_i \right \|^2 = ( R x_i - y_i)^{T}( R x_i - y_i) = (x_i^T R^T - y_i^T)( R x_i - y_i) \\ = x_i^T R^T R x_i - x_i^T R^T y_i - y_i^T R x_i + y_i^T y_i \\ = x_i^T x_i + y_i^T y_i - 2 y_i^T R x_i \] 

將負號考慮進去後把此問題轉換成求最大值的問題： \[ R = arg\ min\ \sum_{i=1}^{n} w_i \left \| R x_i - y_i \right \|^2 \\ = arg\ max\ \sum_{i=1}^{n} w_i y_i^T R x_i \] 

利用矩陣 trace 的性質將此最大值問題轉換為找出最大的 trace，其中 X, Y 為所有 \(x_i, y_i\) 構成的矩陣：\[ \sum_{i=1}^{n} w_i y_i^T R x_i = tr(WY^T R X)\] 

接下來利用矩陣 trace 的性質 tr(AB) = tr(BA)：\[ tr(WY^T R X) = tr(R XWY^T)\]

SVD 來解 \(R\)

令 \(H = XWY^T\)，而我們針對 \(H\) 計算 SVD： \[ H = U \Lambda V^T \] 則此最佳的旋轉矩陣 \(R\) 的解就是 \(V U^T\)。 

證明 \(V U^T\) 是最佳的解

首先要用到一個引理—任何的正定矩陣 \(AA^T\) 及 orthonormal 矩陣 \(B\)，則： \[ tr(AA^T)\geq tr(BAA^T) \] 證明如下：\[tr(BAA^T) = tr(A^T BA) = \sum_i a_i^T (B a_i)\] 但根據 Cauchy–Schwarz inequality： \[ a_i^T (B a_i) \leq \sqrt{(a_i^Ta_i)(a_i^TB^TBa_i)}=a_i^Ta_i \] 因此得證。 

我們將 \(R = VU^T\) 代回原式可得： \[ RH = VU^TH = VU^TU \Lambda V^T = V \Lambda V^T \] \(V \Lambda V^T\) 是正定矩陣，因此無法找到任何的 orthonormal 矩陣 X 使得 \(tr(X V \Lambda V^T)\) 大於 \(tr(V \Lambda V^T)\)，而旋轉矩陣又一定是 orthonormal 矩陣，因此 \(R = VU^T\) 必定是最佳解。

反射修正

如果求出來的 R 的行列式為 -1 的話，代表解出了一個反射矩陣而不是旋轉矩陣，因此我們得修正 \(R\) 的解： \[ R = V \begin{bmatrix} 1 & & & \\ & 1 & & \\ & & \ddots & \\ & & & det(VU^T) \end{bmatrix} U^T \]

參考資料

[1] Least-Squares Fitting of Two 3-D Point Sets

[2] 使用 SVD 方法求解 ICP 问题