Generalized-ICP(GICP)論文研讀

Generalized-ICP論文研讀

• 前言
• 損失函數推導
• 應用
• • point-to-point
  • point-to-plane
  • plane-to-plane

前言

ICP最基本的形式是point-to-point，即以點到點之間的距離作為損失函數；它的一個變種是point-to-plane，改用點到目標點局部擬合平面的距離作為損失函數。

本篇介紹的GICP是上述兩者的generalization，它重新定義了自己的損失函數。point-to-point，point-to-plane，甚至plane-to-plane都可以用GICP這個統一的框架表達。

從後文可以看到，GICP是在最小化的步驟中加入了一個機率模型。
但要注意的是，它使用的配對估計方式仍是點對在歐式空間中的距離，而非基於機率的距離度量方式。

本篇僅關注論文的第III章，並補全論文中省略的公式推導。

損失函數推導

假設我們有兩配對好的點雲 A = { a i } i = 1 , 2 , . . . N A = \{a_i\}_{i=1,2,...N} A={ai​}i=1,2,...N​和 B = { b i } i = 1 , 2 , . . . N B = \{b_i\}_{i=1,2,...N} B={bi​}i=1,2,...N​，其中$a_i$及$b_i$兩兩配對。

GICP論文中做了一個假設，即$A,B$兩點雲是分別由底層的點雲 A ^ = { a i ^ } \hat{A} = \{\hat{a_i}\} A^={ai​^​}和 B ^ = { b i ^ } \hat{B} = \{\hat{b_i}\} B^={bi​^​}依照高斯機率模型$a_i\sim \mathcal{N}(\widehat{a_i},C_i^A)$和$b_i\sim \mathcal{N}(\widehat{b_i},C_i^B)$採樣而來。

${\mathbf{T}}^{\ast }$（注意有上標${}^{\ast }$）是底層兩點雲真實的轉換關係：$\widehat{b_i}={\mathbf{T}}^{\ast }\widehat{a_i}$。我們需要一個目標函數才能使用優化方法尋找最佳的${\mathbf{T}}^{\ast }$，以下就是目標函數推導的過程。

首先定義$d_i^{(\mathbf{T})}$如下，即對原始點雲使用$\mathbf{T}$做轉換後，第$i$個點對的有向距離：

$$d_i^{(\mathbf{T})}\triangleq b_i-\mathbf{T}{a}_i,\forall \text{ rigid transformation }\mathbf{T}$$

它是由以下分布採樣而來：

$$\begin{array}{rl}{d}_i^{(\mathbf{T})}& \sim \mathcal{N}(\widehat{b_i},C_i^B)-\mathbf{T}\mathcal{N}(\widehat{a_i},C_i^A)\\ & =\mathcal{N}(\widehat{b_i}-\mathbf{T}\widehat{a_i},C_i^B+(\mathbf{T})C_i^A(\mathbf{T}{)}^T)\end{array}$$

其中等號參考兩獨立高斯隨機變數之和。

如果將$\mathbf{T}$替換成${\mathbf{T}}^{\ast }$，則有以下關係：

$$\begin{array}{rl}{d}_i^{({\mathbf{T}}^{\ast })}& \sim \mathcal{N}(\widehat{b_i},C_i^B)-{\mathbf{T}}^{\ast }\mathcal{N}(\widehat{a_i},C_i^A)\\ & =\mathcal{N}(\widehat{b_i}-({\mathbf{T}}^{\ast })\widehat{a_i},C_i^B+({\mathbf{T}}^{\ast })C_i^A({\mathbf{T}}^{\ast }{)}^T)\\ & =\mathcal{N}(0,C_i^B+({\mathbf{T}}^{\ast })C_i^A({\mathbf{T}}^{\ast }{)}^T)\end{array}$$

使用MLE最大似然估計，尋找一個使得當前樣本$d_i$出現概率最大的$\mathbf{T}$：

$$\begin{array}{rlrl}\mathbf{T}& =\underset{\mathbf{T}}{\mathrm{argmax}}\prod _{i}p(d_i^{(\mathbf{T})})& & \\ & =\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i\log (p(d_i^{(\mathbf{T})}))& & \text{取log}\\ & =\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i\log (\frac{1}{\sqrt{(2\pi {)}^k|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|}})& & \\ & -\frac{1}{2}(d_i^{(\mathbf{T})}-(\widehat{b_i}-\mathbf{T}\widehat{a_i}){)}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}(d_i^{(\mathbf{T})}-(\widehat{b_i}-\mathbf{T}\widehat{a_i}))& & \text{見註一}\\ & =\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i\log (\frac{1}{\sqrt{(2\pi {)}^k|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|}})& & \\ & -\frac{1}{2}{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}& & \text{見註二}\\ & =\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i-\frac{1}{2}{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}& & \text{見註三}\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum _i\frac{1}{2}{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}& & \\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum _i{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}& & \end{array}$$

最後可以得到論文中的公式(2)：

$$\mathbf{T}=\underset{\mathbf{T}}{\mathrm{argmin}}\sum {d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}$$

註一：
參考Multivariate normal distribution，對於多元常態分布$\text{X}\sim \mathcal{N}(\mu ,\Sigma )$，其機率密度函數(pdf)的公式如下：
$$f_x(x_1,...,x_k)=\frac{1}{\sqrt{(2\pi {)}^k|\Sigma |}}{e}^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )},|\Sigma |\triangleq \text{det}\Sigma$$

對它取$\log$可以得到：

$$\begin{array}{rl}\log (f_x(x_1,...,x_k))& =\log (\frac{1}{\sqrt{(2\pi {)}^k|\Sigma |}}{e}^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )})\\ & =\log (\frac{1}{\sqrt{(2\pi {)}^k|\Sigma |}})+\log (e^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )})\\ & =\log (\frac{1}{\sqrt{(2\pi {)}^k|\Sigma |}})-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\end{array}$$

代入$d_i^{(\mathbf{T})}\sim \mathcal{N}(\widehat{b_i}-\mathbf{T}\widehat{a_i},C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T)$，得：

$$\begin{array}{rl}\log (p(d_i^{(\mathbf{T})}))& =\log (\frac{1}{\sqrt{(2\pi {)}^k|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|}})\\ & -\frac{1}{2}(d_i^{(\mathbf{T})}-(\widehat{b_i}-\mathbf{T}\widehat{a_i}){)}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}(d_i^{(\mathbf{T})}-(\widehat{b_i}-\mathbf{T}\widehat{a_i}))\\ & =\log (\frac{1}{\sqrt{(2\pi {)}^k|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|}})\\ & -\frac{1}{2}{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}\end{array}$$

註二：
在$\mathbf{T}={\mathbf{T}}^{\ast }$的情況下$\widehat{b_i}-({\mathbf{T}}^{\ast })\widehat{a_i}=0$，但是可以這樣假設？

註三：
旋轉矩陣判別式為1，平移矩陣判別式為1。又因為$\mathbf{T}$是旋轉平移矩陣，可以拆成旋轉矩陣與平移矩陣的乘積，且$\text{det}(AB)=\text{det}(A)\text{det}(B)$，所以有$\text{det}(\mathbf{T})=1$，因此$\text{det}(\mathbf{T}{C}_i^A{\mathbf{T}}^T)=\text{det}(C_i^A)$。
但是$|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|\ne |C_i^B|+|\mathbf{T}{C}_i^A{\mathbf{T}}^T|$，有辦法推出第一項和$\mathbf{T}$無關?可參考Expressing the determinant of a sum of two matrices?

或照視覺十四講所說，如果是對$d_i$做優化，第一項就變為常數，可以忽略。但是此處是對$T$做優化，可以套用這個理論?

應用

GICP統一了point-to-point和point-to-plane，甚至還納入了plane-to-plane。這幾種變形的差別在於共變異數矩陣$C_i^A,C_i^B$的選取。

point-to-point

傳統點到點的ICP可以用GICP的框架表示如下：
$$C_i^B=I,C_i^A=0$$

驗證：
$$\begin{array}{rl}\mathbf{T}& =\underset{\mathbf{T}}{\mathrm{argmin}}\sum {d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum {d_i^{(\mathbf{T})}}^T{d}_i^{(\mathbf{T})}\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum \Vert d_i^{(\mathbf{T})}{\Vert }^2\end{array}$$

可以看出其目標為最小化點對間距離平方之和，也就是點到點ICP的更新公式。

point-to-plane

首先定義一個為正交投影矩陣${\mathbf{P}}_{\mathbf{i}}$，有以下性質：${\mathbf{P}}_{\mathbf{i}}={{\mathbf{P}}_{\mathbf{i}}}^2={{\mathbf{P}}_{\mathbf{i}}}^T$。
${\mathbf{P}}_{\mathbf{i}}$會將向量投影到目標點雲中第$i$點$b_i$法向量的span上，因此${\mathbf{P}}_{\mathbf{i}}\cdot d_i^{(\mathbf{T})}$也就是轉換後的$a_i$到$b_i$所在平面的距離。

point-to-plane ICP的更新公式可以表示如下：

T = arg min ⁡ T { ∑ i ∥ P i ⋅ d i ( T ) ∥ 2 } = arg min ⁡ T { ∑ i ( P i ⋅ d i ( T ) ) T ( P i ⋅ d i ( T ) ) } = arg min ⁡ T { ∑ i d i ( T ) T ⋅ P i 2 ⋅ d i ( T ) } = arg min ⁡ T { ∑ i d i ( T ) T ⋅ P i ⋅ d i ( T ) } \begin{aligned}\bold{T} &=\argmin\limits_\bold{T} \{\sum\limits_i \|\bold{P_i} \cdot d_i^{(\bold{T})}\|^2\} \\&= \argmin\limits_\bold{T} \{\sum\limits_i (\bold{P_i} \cdot d_i^{(\bold{T})})^T(\bold{P_i} \cdot d_i^{(\bold{T})})\} \\&= \argmin\limits_\bold{T} \{\sum\limits_i{d_i^{(\bold{T})}}^T \cdot \bold{P_i}^2 \cdot d_i^{(\bold{T})}\} \\&= \argmin\limits_\bold{T} \{\sum\limits_i{d_i^{(\bold{T})}}^T \cdot \bold{P_i} \cdot d_i^{(\bold{T})}\}\end{aligned} T​=Targmin​{i∑​∥Pi​⋅di(T)​∥2}=Targmin​{i∑​(Pi​⋅di(T)​)T(Pi​⋅di(T)​)}=Targmin​{i∑​di(T)​T⋅Pi​2⋅di(T)​}=Targmin​{i∑​di(T)​T⋅Pi​⋅di(T)​}​

與GICP的公式相比較，可以發現以下關係：

$$C_i^B={{\mathbf{P}}_{\mathbf{i}}}^{-1},C_i^A=0$$

Note: ${\mathbf{P}}_{\mathbf{i}}$需要被近似?待補

plane-to-plane

可以把真實世界中的物體看作是分段線性的，而相機在對物體進行掃描時，是對該物體做採樣。可以想見，從不同角度拍攝物體，相機所採樣的點不一定相同。採樣點在局部擬合平面方向上的不確定性較大，在法向量方向上的不確定性較小。

假設局部擬合平面上某一點的法向量是x軸方向，那麼點的共變異數矩陣可以表示為：

$$\left[\begin{array}{ccc}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

其中$ϵ$是一個極小的數。

因為實際上法向量並不一定是沿x軸方向，所以需要進行座標轉換。
假設$b_i$的法向量為$u_i$，$a_i$的法向量為$v_i$，那麼它們各自的共變異數矩陣分別為：

$$C_i^B={\mathbf{R}}_{u_i}\left[\begin{array}{ccc}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\mathbf{R}}_{u_i}^T$$

$$C_i^A={\mathbf{R}}_{v_i}\left[\begin{array}{ccc}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\mathbf{R}}_{v_i}^T$$

其中${\mathbf{R}}_x$為一將$e_1$轉成$x$的旋轉矩陣。

為什麼是前後都乘旋轉矩陣呢？套用共變異數矩陣的定義就明白了：

$$\begin{array}{rlrl}{C}_i^B& ={\mathbf{R}}_{u_i}\mathrm{Cov}(\text{X}){\mathbf{R}}_{u_i}^T& & \\ & ={\mathbf{R}}_{u_i}E[(\text{X}-E[\text{X}])(\text{X}-E[\text{X}]{)}^T]{\mathbf{R}}_{u_i}^T& & \\ & =E[({\mathbf{R}}_{u_i}\text{X}-E[{\mathbf{R}}_{u_i}\text{X}])({\mathbf{R}}_{u_i}\text{X}-E[{\mathbf{R}}_{u_i}\text{X}]{)}^T]& & \\ & =E[(\text{U}-E[\text{U}])(\text{U}-E[\text{U}]{)}^T]& & U\triangleq {\mathbf{R}}_{u_i}\text{X}\end{array}$$

$\mathrm{Cov}(\text{X})=\left[\begin{array}{ccc}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$代表x方向不確定性很小的共變異數矩陣，上式中對不確定性很小的方向做了旋轉($U\triangleq {\mathbf{R}}_{u_i}\text{X}$)，所以$C_i^B$是一個在$u_i$方向上不確定性很小的共變異數矩陣。

Note:$R$的取法待補
Q:為何共變異數矩陣對角線上的值是$ϵ,1,1$?有需要做縮放?