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={ai}i=1,2,...NA = \{a_i\}_{i=1,2,...N}A={ai}i=1,2,...N和B={bi}i=1,2,...NB = \{b_i\}_{i=1,2,...N}B={bi}i=1,2,...N，其中aia_iai及bib_ibi兩兩配對。

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

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

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

di(T)≜bi−Tai,∀rigid transformation Td_i^{(\bold{T})} \triangleq b_i-\bold{T}a_i, \forall \text{ rigid transformation } \bold{T}di(T)≜bi−Tai,∀ rigid transformation T

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

di(T)∼N(bi^,CiB)−TN(ai^,CiA)=N(bi^−Tai^,CiB+(T)CiA(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∗，則有以下關係：

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)\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最大似然估計，尋找一個使得當前樣本did_idi出現概率最大的T\bold{T}T：

T=arg max⁡T∏ip(di(T))=arg max⁡T∑ilog⁡(p(di(T)))取log=arg max⁡T∑ilog⁡(1(2π)k∣CiB+TCiATT∣)−12(di(T)−(bi^−Tai^))T(CiB+TCiATT)−1(di(T)−(bi^−Tai^))見註一=arg max⁡T∑ilog⁡(1(2π)k∣CiB+TCiATT∣)−12di(T)T(CiB+TCiATT)−1di(T)見註二=arg max⁡T∑i−12di(T)T(CiB+TCiATT)−1di(T)見註三=arg min⁡T∑i12di(T)T(CiB+TCiATT)−1di(T)=arg min⁡T∑idi(T)T(CiB+TCiATT)−1di(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∑di(T)T(CiB+TCiATT)−1di(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)的公式如下：
fx(x1,...,xk)=1(2π)k∣Σ∣e−12(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} \Sigmafx(x1,...,xk)=(2π)k∣Σ∣1e−21(x−μ)TΣ−1(x−μ),∣Σ∣≜detΣ

對它取log⁡\loglog可以得到：

log⁡(fx(x1,...,xk))=log⁡(1(2π)k∣Σ∣e−12(x−μ)TΣ−1(x−μ))=log⁡(1(2π)k∣Σ∣)+log⁡(e−12(x−μ)TΣ−1(x−μ))=log⁡(1(2π)k∣Σ∣)−12(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−μ)

代入di(T)∼N(bi^−Tai^,CiB+TCiATT)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(di(T)))=log⁡(1(2π)k∣CiB+TCiATT∣)−12(di(T)−(bi^−Tai^))T(CiB+TCiATT)−1(di(T)−(bi^−Tai^))=log⁡(1(2π)k∣CiB+TCiATT∣)−12di(T)T(CiB+TCiATT)−1di(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∗的情況下bi^−(T∗)ai^=0\hat{b_i} - (\bold{T}^*)\hat{a_i} =0bi^−(T∗)ai^=0，但是可以這樣假設？

註三：
旋轉矩陣判別式為1，平移矩陣判別式為1。又因為T\bold{T}T是旋轉平移矩陣，可以拆成旋轉矩陣與平移矩陣的乘積，且det(AB)=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}) = 1det(T)=1，因此det(TCiATT)=det(CiA)\textbf{det}(\bold{T}C_i^A\bold{T}^T)=\textbf{det}(C_i^A)det(TCiATT)=det(CiA)。
但是∣CiB+TCiATT∣≠∣CiB∣+∣TCiATT∣|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?

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

應用

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

point-to-point

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

驗證：
T=arg min⁡T∑di(T)T(CiB+TCiATT)−1di(T)=arg min⁡T∑di(T)Tdi(T)=arg min⁡T∑∥di(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

首先定義一個為正交投影矩陣Pi\bold{P_i}Pi，有以下性質：Pi=Pi2=PiT\bold{P_i} = \bold{P_i}^2 = \bold{P_i} ^TPi=Pi2=PiT。
Pi\bold{P_i}Pi會將向量投影到目標點雲中第iii點bib_ibi法向量的span上，因此Pi⋅di(T)\bold{P_i}\cdot d_i^{(\bold{T})}Pi⋅di(T)也就是轉換後的aia_iai到bib_ibi所在平面的距離。

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

T=arg min⁡T{∑i∥Pi⋅di(T)∥2}=arg min⁡T{∑i(Pi⋅di(T))T(Pi⋅di(T))}=arg min⁡T{∑idi(T)T⋅Pi2⋅di(T)}=arg min⁡T{∑idi(T)T⋅Pi⋅di(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的公式相比較，可以發現以下關係：

CiB=Pi−1,CiA=0C_i^B=\bold{P_i}^{-1}, C_i^A=0CiB=Pi−1,CiA=0

Note: Pi\bold{P_i}Pi需要被近似?待補

plane-to-plane

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

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

[ϵ00010001]\begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}⎣⎡ϵ00010001⎦⎤

其中ϵ\epsilonϵ是一個極小的數。

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

CiB=Rui[ϵ00010001]RuiTC_i^B=\bold{R}_{u_i} \begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}\bold{R}_{u_i}^TCiB=Rui⎣⎡ϵ00010001⎦⎤RuiT

CiA=Rvi[ϵ00010001]RviTC_i^A=\bold{R}_{v_i} \begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}\bold{R}_{v_i}^TCiA=Rvi⎣⎡ϵ00010001⎦⎤RviT

其中Rx\bold{R}_{x}Rx為一將e1e_1e1轉成xxx的旋轉矩陣。

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

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\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)=[ϵ00010001]\operatorname{Cov}(\textbf{X})=\begin{bmatrix}\epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}Cov(X)=⎣⎡ϵ00010001⎦⎤代表x方向不確定性很小的共變異數矩陣，上式中對不確定性很小的方向做了旋轉(U≜RuiXU \triangleq \bold{R}_{u_i}\textbf{X}U≜RuiX)，所以CiBC_i^BCiB是一個在uiu_iui方向上不確定性很小的共變異數矩陣。

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

Generalized-ICP(GICP)論文研讀相关推荐

TensorRT/samples/common/argsParser.h源碼研讀
TensorRT/samples/common/argsParser.h源碼研讀 argsParser.h namespace struct的繼承 caffe特有參數 UFF格式不需要typedef ...
PCL - MLS代碼研讀（十五）- VOXEL_GRID_DILATION上採樣方法
PCL - MLS代碼研讀(十五)- VOXEL_GRID_DILATION上採樣方法前言成員變數 MLSVoxelGrid MLSVoxelGrid建構子 dilate函數 getter &am ...
java a3 套打印_Java - apache PDFBox兩個A3論文到一個A2？
您可能希望它根據其的JavaDoc看PageCombinationSample.java確實幾乎你所需要的: 此示例演示如何多頁合併成單一的大頁(例如兩張A4模塊到一個A3模塊)使用表單XObjec ...
P3P Kneip - A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation
P3P Kneip - A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation ...
(原創) 為什麼企業研發喜歡找研究生? (日記)
我常常在想,大學生跟研究生有什麼差別?為什麼研發喜歡找研究生呢? 首先看看大學生怎麼唸書,大學生基本上修的課都有課本,也就是說,只要把課本念好搞懂,基本上就沒問題了,而課本都是大師整理好的東西,有範例 ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
涨点神器！南航提出AFF：注意力特征融合，即插即用！可用于分类、检测和分割等...
点击下方卡片,关注"CVer"公众号 AI/CV重磅干货,第一时间送达作者:OucQxw | 已授权转载(源:知乎) https://zhuanlan.zhihu.com/p ...
Real-time hatching報告+實現代碼和效果
1.論文解讀論文標題為實時的畫影線,實時的畫影線的主要作用可以用於產生素描畫的效果.該論文屬於計算機圖形學中的圖像處理技術和紋理技術. 根據摘要,論文可以被分為四個部分.該論文通過分析已有技術中的問 ...
ICP、Point-to-plane ICP、GICP以及VGICP方法介绍
ICP.Point-to-plane ICP.GICP以及VGICP方法介绍引言如有错误或需完善部分请留言,有最新的论文烦请留言告知,会进一步完善,感谢支持. 这篇博客系统的介绍ICP(itera ...

Generalized-ICP(GICP)論文研讀