Generalized-ICP論文研讀
前言
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 a_i ai及 b i b_i bi兩兩配對。
GICP論文中做了一個假設，即 A , B A,B A,B兩點雲是分別由底層的點雲 A ^ = { a i ^ } \hat{A} = \{\hat{a_i}\} A^={ai^}和 B ^ = { b i ^ } \hat{B} = \{\hat{b_i}\} B^={bi^}依照高斯機率模型 a i ∼ N ( a i ^ , C i A ) a_i \sim \mathcal{N}(\hat{a_i},C_i^A) ai∼N(ai^,CiA)和 b i ∼ N ( b i ^ , C i B ) b_i \sim \mathcal{N}(\hat{b_i},C_i^B) bi∼N(bi^,CiB)採樣而來。
T ∗ \bold{T}^* T∗（注意有上標 ∗ ^* ∗）是底層兩點雲真實的轉換關係： b i ^ = T ∗ a i ^ \hat{b_i} = \bold{T}^*\hat{a_i} bi^=T∗ai^。我們需要一個目標函數才能使用優化方法尋找最佳的 T ∗ \bold{T}^* T∗，以下就是目標函數推導的過程。
首先定義 d i ( T ) d_i^{(\bold{T})} di(T)如下，即對原始點雲使用 T \bold{T} T做轉換後，第 i i i個點對的有向距離：
d i ( T ) ≜ b i − T a i , ∀ rigid transformation T d_i^{(\bold{T})} \triangleq b_i-\bold{T}a_i, \forall \text{ rigid transformation } \bold{T} di(T)≜bi−Tai,∀ rigid transformation T
它是由以下分布採樣而來：
d i ( T ) ∼ N ( b i ^ , C i B ) − T N ( a i ^ , C i A ) = N ( b i ^ − T a i ^ , C i B + ( T ) C i A ( T ) T ) \begin{aligned}d_i^{(\bold{T})} &\sim \mathcal{N}(\hat{b_i},C_i^B) - \bold{T}\mathcal{N}(\hat{a_i},C_i^A) \\&= \mathcal{N}(\hat{b_i} - \bold{T}\hat{a_i}, C_i^B+(\bold{T})C_i^A(\bold{T})^T)\end{aligned} di(T)∼N(bi^,CiB)−TN(ai^,CiA)=N(bi^−Tai^,CiB+(T)CiA(T)T)
其中等號參考兩獨立高斯隨機變數之和。
如果將 T \bold{T} T替換成 T ∗ \bold{T}^* T∗，則有以下關係：
d i ( T ∗ ) ∼ N ( b i ^ , C i B ) − T ∗ N ( a i ^ , C i A ) = N ( b i ^ − ( T ∗ ) a i ^ , C i B + ( T ∗ ) C i A ( T ∗ ) T ) = N ( 0 , C i B + ( T ∗ ) C i A ( T ∗ ) T ) \begin{aligned}d_i^{(\bold{T}^*)} &\sim \mathcal{N}(\hat{b_i},C_i^B) - \bold{T}^*\mathcal{N}(\hat{a_i},C_i^A) \\&= \mathcal{N}(\hat{b_i} - (\bold{T}^*)\hat{a_i}, C_i^B+(\bold{T}^*)C_i^A(\bold{T}^*)^T)\\ &= \mathcal{N}(0, C_i^B+(\bold{T}^*)C_i^A(\bold{T}^*)^T)\end{aligned} di(T∗)∼N(bi^,CiB)−T∗N(ai^,CiA)=N(bi^−(T∗)ai^,CiB+(T∗)CiA(T∗)T)=N(0,CiB+(T∗)CiA(T∗)T)
使用MLE最大似然估計，尋找一個使得當前樣本 d i d_i di出現概率最大的 T \bold{T} T：
T = arg max T ∏ i p ( d i ( T ) ) = arg max T ∑ i log ( p ( d i ( T ) ) ) 取log = arg max T ∑ i log ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 ( d i ( T ) − ( b i ^ − T a i ^ ) ) T ( C i B + T C i A T T ) − 1 ( d i ( T ) − ( b i ^ − T a i ^ ) ) 見註一 = arg max T ∑ i log ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) 見註二 = arg max T ∑ i − 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) 見註三 = arg min T ∑ i 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) = arg min T ∑ i d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) \begin{aligned}\bold{T} &= \argmax\limits_\bold{T} \prod\limits_ip(d_i^{(\bold{T})}) \\&= \argmax\limits_\bold{T} \sum\limits_i\log (p(d_i^{(\bold{T})})) && \text{取log} \\&= \argmax\limits_\bold{T} \sum\limits_i\log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\&-\frac{1}{2}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i}))^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i}))&& \text{見註一} \\&= \argmax\limits_\bold{T} \sum\limits_i\log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\&-\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}&& \text{見註二} \\&=\argmax\limits_\bold{T}\sum\limits_i-\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}&& \text{見註三} \\&= \argmin\limits_\bold{T}\sum\limits_i\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})} \\&= \argmin\limits_\bold{T}\sum\limits_i{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})} \end{aligned} T=Targmaxi∏p(di(T))=Targmaxi∑log(p(di(T)))=Targmaxi∑log((2π)k∣CiB+TCiATT∣ 1)−21(di(T)−(bi^−Tai^))T(CiB+TCiATT)−1(di(T)−(bi^−Tai^))=Targmaxi∑log((2π)k∣CiB+TCiATT∣ 1)−21di(T)T(CiB+TCiATT)−1di(T)=Targmaxi∑−21di(T)T(CiB+TCiATT)−1di(T)=Targmini∑21di(T)T(CiB+TCiATT)−1di(T)=Targmini∑di(T)T(CiB+TCiATT)−1di(T)取log見註一見註二見註三
最後可以得到論文中的公式(2)：
T = arg min T ∑ d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) \bold{T} = \argmin\limits_\bold{T} \sum\limits {d_i^{(\bold{T})}}^T (C_i^B + \bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})} T=Targmin∑di(T)T(CiB+TCiATT)−1di(T)
註一：
參考Multivariate normal distribution，對於多元常態分布
X
∼
N
(
μ
,
Σ
)
\textbf{X} \sim \mathcal{N}(\mu, \Sigma)
X∼N(μ,Σ)，其機率密度函數(pdf)的公式如下：
f
x
(
x
1
,
.
.
.
,
x
k
)
=
1
(
2
π
)
k
∣
Σ
∣
e
−
1
2
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
,
∣
Σ
∣
≜
det
Σ
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 \textbf{det} \Sigma
fx(x1,...,xk)=(2π)k∣Σ∣
1e−21(x−μ)TΣ−1(x−μ),∣Σ∣≜detΣ
對它取 log \log log可以得到：
log ( f x ( x 1 , . . . , x k ) ) = log ( 1 ( 2 π ) k ∣ Σ ∣ e − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) = log ( 1 ( 2 π ) k ∣ Σ ∣ ) + log ( e − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) = log ( 1 ( 2 π ) k ∣ Σ ∣ ) − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) \begin{aligned} \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{aligned} log(fx(x1,...,xk))=log((2π)k∣Σ∣ 1e−21(x−μ)TΣ−1(x−μ))=log((2π)k∣Σ∣ 1)+log(e−21(x−μ)TΣ−1(x−μ))=log((2π)k∣Σ∣ 1)−21(x−μ)TΣ−1(x−μ)
代入 d i ( T ) ∼ N ( b i ^ − T a i ^ , C i B + T C i A T T ) d_i^{(\bold{T})} \sim \mathcal{N}(\hat{b_i} - \bold{T}\hat{a_i}, C_i^B+\bold{T}C_i^A\bold{T}^T) di(T)∼N(bi^−Tai^,CiB+TCiATT)，得：
log ( p ( d i ( T ) ) ) = log ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 ( d i ( T ) − ( b i ^ − T a i ^ ) ) T ( C i B + T C i A T T ) − 1 ( d i ( T ) − ( b i ^ − T a i ^ ) ) = log ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) \begin{aligned}\log (p(d_i^{(\bold{T})})) &= \log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\&-\frac{1}{2}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i}))^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i})) \\&= \log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\&-\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}\end{aligned} log(p(di(T)))=log((2π)k∣CiB+TCiATT∣ 1)−21(di(T)−(bi^−Tai^))T(CiB+TCiATT)−1(di(T)−(bi^−Tai^))=log((2π)k∣CiB+TCiATT∣ 1)−21di(T)T(CiB+TCiATT)−1di(T)
註二：
在
T
=
T
∗
\bold{T}=\bold{T}^*
T=T∗的情況下
b
i
^
−
(
T
∗
)
a
i
^
=
0
\hat{b_i} - (\bold{T}^*)\hat{a_i} =0
bi^−(T∗)ai^=0，但是可以這樣假設？
註三：
旋轉矩陣判別式為1，平移矩陣判別式為1。又因為
T
\bold{T}
T是旋轉平移矩陣，可以拆成旋轉矩陣與平移矩陣的乘積，且
det
(
A
B
)
=
det
(
A
)
det
(
B
)
\textbf{det}(AB) = \textbf{det}(A)\textbf{det}(B)
det(AB)=det(A)det(B)，所以有
det
(
T
)
=
1
\textbf{det}(\bold{T}) = 1
det(T)=1，因此
det
(
T
C
i
A
T
T
)
=
det
(
C
i
A
)
\textbf{det}(\bold{T}C_i^A\bold{T}^T)=\textbf{det}(C_i^A)
det(TCiATT)=det(CiA)。
但是
∣
C
i
B
+
T
C
i
A
T
T
∣
≠
∣
C
i
B
∣
+
∣
T
C
i
A
T
T
∣
|C_i^B+\bold{T}C_i^A\bold{T}^T| \neq |C_i^B|+|\bold{T}C_i^A\bold{T}^T|
∣CiB+TCiATT∣=∣CiB∣+∣TCiATT∣，有辦法推出第一項和
T
\bold{T}
T無關?可參考Expressing the determinant of a sum of two matrices?
或照視覺十四講所說，如果是對 d i d_i di做優化，第一項就變為常數，可以忽略。但是此處是對 T T T做優化，可以套用這個理論?
應用
GICP統一了point-to-point和point-to-plane，甚至還納入了plane-to-plane。這幾種變形的差別在於共變異數矩陣 C i A , C i B C_i^A,C_i^B CiA,CiB的選取。
point-to-point
傳統點到點的ICP可以用GICP的框架表示如下：
C
i
B
=
I
,
C
i
A
=
0
C_i^B=I, C_i^A=0
CiB=I,CiA=0
驗證：
T
=
arg min
T
∑
d
i
(
T
)
T
(
C
i
B
+
T
C
i
A
T
T
)
−
1
d
i
(
T
)
=
arg min
T
∑
d
i
(
T
)
T
d
i
(
T
)
=
arg min
T
∑
∥
d
i
(
T
)
∥
2
\begin{aligned}\bold{T} &= \argmin\limits_\bold{T} \sum\limits {d_i^{(\bold{T})}}^T (C_i^B + \bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})} \\ &= \argmin\limits_\bold{T} \sum\limits {d_i^{(\bold{T})}}^T d_i^{(\bold{T})} \\ &= \argmin\limits_\bold{T} \sum\limits {\|d_i^{(\bold{T})}\|^2}\end{aligned}
T=Targmin∑di(T)T(CiB+TCiATT)−1di(T)=Targmin∑di(T)Tdi(T)=Targmin∑∥di(T)∥2
可以看出其目標為最小化點對間距離平方之和，也就是點到點ICP的更新公式。
point-to-plane
首先定義一個為正交投影矩陣
P
i
\bold{P_i}
Pi，有以下性質：
P
i
=
P
i
2
=
P
i
T
\bold{P_i} = \bold{P_i}^2 = \bold{P_i} ^T
Pi=Pi2=PiT。
P
i
\bold{P_i}
Pi會將向量投影到目標點雲中第
i
i
i點
b
i
b_i
bi法向量的span上，因此
P
i
⋅
d
i
(
T
)
\bold{P_i}\cdot d_i^{(\bold{T})}
Pi⋅di(T)也就是轉換後的
a
i
a_i
ai到
b
i
b_i
bi所在平面的距離。
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⋅Pi2⋅di(T)}=Targmin{i∑di(T)T⋅Pi⋅di(T)}
與GICP的公式相比較，可以發現以下關係：
C i B = P i − 1 , C i A = 0 C_i^B=\bold{P_i}^{-1}, C_i^A=0 CiB=Pi−1,CiA=0
Note: P i \bold{P_i} Pi需要被近似?待補
plane-to-plane
可以把真實世界中的物體看作是分段線性的，而相機在對物體進行掃描時，是對該物體做採樣。可以想見，從不同角度拍攝物體，相機所採樣的點不一定相同。採樣點在局部擬合平面方向上的不確定性較大，在法向量方向上的不確定性較小。
假設局部擬合平面上某一點的法向量是x軸方向，那麼點的共變異數矩陣可以表示為：
[ ϵ 0 0 0 1 0 0 0 1 ] \begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix} ⎣⎡ϵ00010001⎦⎤
其中 ϵ \epsilon ϵ是一個極小的數。
因為實際上法向量並不一定是沿x軸方向，所以需要進行座標轉換。
假設
b
i
b_i
bi的法向量為
u
i
u_i
ui，
a
i
a_i
ai的法向量為
v
i
v_i
vi，那麼它們各自的共變異數矩陣分別為：
C i B = R u i [ ϵ 0 0 0 1 0 0 0 1 ] R u i T C_i^B=\bold{R}_{u_i} \begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}\bold{R}_{u_i}^T CiB=Rui⎣⎡ϵ00010001⎦⎤RuiT
C i A = R v i [ ϵ 0 0 0 1 0 0 0 1 ] R v i T C_i^A=\bold{R}_{v_i} \begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}\bold{R}_{v_i}^T CiA=Rvi⎣⎡ϵ00010001⎦⎤RviT
其中 R x \bold{R}_{x} Rx為一將 e 1 e_1 e1轉成 x x x的旋轉矩陣。
為什麼是前後都乘旋轉矩陣呢？套用共變異數矩陣的定義就明白了：
C i B = R u i Cov ( X ) R u i T = R u i E [ ( X − E [ X ] ) ( X − E [ X ] ) T ] R u i T = E [ ( R u i X − E [ R u i X ] ) ( R u i X − E [ R u i X ] ) T ] = E [ ( U − E [ U ] ) ( U − E [ U ] ) T ] U ≜ R u i X \begin{aligned}C_i^B&=\bold{R}_{u_i}\operatorname{Cov}(\textbf{X})\bold{R}_{u_i}^T \\&= \bold{R}_{u_i}\operatorname{E}[(\textbf{X}-\operatorname{E}[\textbf{X}])(\textbf{X}-\operatorname{E}[\textbf{X}])^T]\bold{R}_{u_i}^T \\&= \operatorname{E}[(\bold{R}_{u_i}\textbf{X}-\operatorname{E}[\bold{R}_{u_i}\textbf{X}])(\bold{R}_{u_i}\textbf{X}-\operatorname{E}[\bold{R}_{u_i}\textbf{X}])^T] \\ &= \operatorname{E}[(\textbf{U}-\operatorname{E}[\textbf{U}])(\textbf{U}-\operatorname{E}[\textbf{U}])^T] && U \triangleq \bold{R}_{u_i}\textbf{X}\end{aligned} CiB=RuiCov(X)RuiT=RuiE[(X−E[X])(X−E[X])T]RuiT=E[(RuiX−E[RuiX])(RuiX−E[RuiX])T]=E[(U−E[U])(U−E[U])T]U≜RuiX
Cov ( X ) = [ ϵ 0 0 0 1 0 0 0 1 ] \operatorname{Cov}(\textbf{X})=\begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix} Cov(X)=⎣⎡ϵ00010001⎦⎤代表x方向不確定性很小的共變異數矩陣，上式中對不確定性很小的方向做了旋轉( U ≜ R u i X U \triangleq \bold{R}_{u_i}\textbf{X} U≜RuiX)，所以 C i B C_i^B CiB是一個在 u i u_i ui方向上不確定性很小的共變異數矩陣。
Note:
R
R
R的取法待補
Q:為何共變異數矩陣對角線上的值是
ϵ
,
1
,
1
\epsilon,1,1
ϵ,1,1?有需要做縮放?