ICP（Iterative Closest Point）算法推導

前言

本篇整理自深藍學院三維點雲處理課程的Lecture 9 – Registration。推導過程中大量地用到了矩陣跡（trace）及Frobenius norm的性質，建議與矩陣的跡（matrix trace）的特性以及證明及Frobenius norm的特性以及證明對照參看。

算法流程

• 尋找(最近鄰)點對，拒絕當中距離過大者
• 估計旋轉矩陣$R$和平移向量$t$，使得 轉換後的原始點雲 與 目標點雲 距離最小化（此即Procrustes Transformation Problem）
• 檢查 損失函數$f$是否夠小 或 $R,t$的變換量是否夠小，若非，則再次執行上述所有步驟

損失函數

兩個已配對的$m$維空間中的點集，$A$和$B$中各有$N$個點：

$A=[a_1,...a_N]\in {\mathbb{R}}^{m\times N}$
$B=[b_1,...b_N]\in {\mathbb{R}}^{m\times N}$

並定義如下矩陣和向量：

$1=[1,...,1{]}^T\in {\mathbb{R}}^{N\times 1}$
$\begin{array}{rlr}R\in {\mathbb{R}}^{m\times m}& & \text{m維空間中的旋轉矩陣}\end{array}$
$\begin{array}{rlr}t\in {\mathbb{R}}^{m\times 1}& & \text{m維空間中的平移向量}\end{array}$

損失函數為：

$f(R,t)=\frac{1}{N}\sum _{i=1}^N\Vert b_i-R{a}_i-t{\Vert }^2=\Vert B-(RA+t{1}^{\mathbf{T}}){\Vert }_F^2$

（最後一項好像少了$\frac{1}{N}$？）

這裡的目標是尋找一組$R$和$t$使得$f$最小化。

求解t

化簡

$\begin{array}{rlrl}f(R,t)& =\Vert B-(RA+t{1}^{\mathbf{T}}){\Vert }_F^2& & \\ & =\Vert (B-RA)+(-t{1}^{\mathbf{T}}){\Vert }_F^2& & \\ & =\Vert B-RA{\Vert }_F^2+\Vert -t{1}^{\mathbf{T}}{\Vert }_F^2+2⟨B-RA,-t{1}^{\mathbf{T}}{⟩}_F& & \text{Frobenius decomposition}\end{array}$

單獨化簡第二項：

$\begin{array}{rlrl}\Vert -t{1}^{\mathbf{T}}{\Vert }_F^2& =\Vert t{1}^{\mathbf{T}}{\Vert }_F^2& & \\ & =\Vert t{{\left[\begin{array}{cccc}1& 1& ...& 1\end{array}\right]}^T}^T{\Vert }_F^2& & \\ & =\Vert \left[\begin{array}{cccc}t& t& ...& t\end{array}\right]{\Vert }_F^2& & \\ & =\text{tr}({\left[\begin{array}{cccc}t& t& ...& t\end{array}\right]}^T\left[\begin{array}{cccc}t& t& ...& t\end{array}\right])& & \text{Forbenius norm：}\Vert A{\Vert }_F=\sqrt{\text{tr}(A{A}^T)}=\sqrt{\text{tr}(A^{T}A)}\\ & =\text{tr}(\left[\begin{array}{cccc}{t}^{T}t& t^{T}t& ...& t^{T}t\\ t^{T}t& t^{T}t& ...& t^{T}t\\ ...& & & \\ t^{T}t& t^{T}t& ...& t^{T}t\end{array}\right])& & \\ & =N{t}^{T}t& & \end{array}$

代回損失函數：

$\begin{array}{rlrl}f(R,t)& =\Vert B-RA{\Vert }_F^2+\Vert -t{1}^{\mathbf{T}}{\Vert }_F^2+2⟨B-RA,-t{1}^{\mathbf{T}}{⟩}_F& & \\ & =\Vert B-RA{\Vert }_F^2+N{t}^{T}t-2⟨B-RA,t{1}^{\mathbf{T}}{⟩}_F& & \\ & =\Vert B-RA{\Vert }_F^2+N{t}^{T}t-2\text{tr}((B-RA)1t^T)& & \text{Frobenius inner product}\end{array}$

令導數為0

將$f$對$t$微分：

$\begin{array}{rlrl}\frac{\partial f}{\partial t}& =\frac{\partial (\Vert B-RA{\Vert }_F^2+N{t}^{T}t-2\text{tr}((B-RA)1t^T))}{\partial t}& & \\ & =\frac{\partial \Vert B-RA{\Vert }_F^2}{\partial t}+\frac{\partial N{t}^{T}t}{\partial t}-2\frac{\partial \text{tr}((B-RA)1t^T)}{\partial t}& & \\ & =0+\frac{\partial N{t}^{T}t}{\partial t}-2\frac{\partial \text{tr}((B-RA)1t^T)}{\partial t}& & \\ & =2Nt-2\frac{\partial \text{tr}((B-RA)1t^T)}{\partial t}& & \frac{\partial x^{T}x}{\partial x}=2x\\ & =2Nt-2(B-RA)1& & \frac{\partial \text{tr}(\mathbf{A}{\mathbf{X}}^T)}{\partial \mathbf{X}}=\mathbf{A}\end{array}$

令$\frac{\partial f}{\partial t}=2Nt-2(B-RA)1$為0。

可以得知取$t=\frac{1}{N}(B-RA)1$時能使得$f$最小化。

註一：$\frac{\partial x^{T}x}{\partial x}$可參考x^Tx對x微分公式證明。

註二：$\frac{\partial \text{tr}(\mathbf{A}{\mathbf{X}}^T)}{\partial \mathbf{X}}=\mathbf{A}$可參考兩矩陣乘積的跡對矩陣微分公式證明。

求解R

回代t

之前化簡得到$f(R,t)=\Vert B-RA{\Vert }_F^2+N{t}^{T}t-2\text{tr}((B-RA)1t^T)$。

將剛剛計算出來的$t=\frac{1}{N}(B-RA)1$代入第二項：

$\begin{array}{rlrl}N{t}^{T}t& =N(\frac{1}{N}{1}^{\mathbf{T}}(B-RA{)}^T)(\frac{1}{N}(B-RA)1)& & \\ & =\frac{1}{N}{1}^{\mathbf{T}}(B-RA{)}^T(B-RA)1& & \\ & =\frac{1}{N}\left[\begin{array}{ccc}1& ...& 1\end{array}\right]\left[\begin{array}{c}(b_1-R{a}_1{)}^T\\ ...\\ (b_N-R{a}_N{)}^T\end{array}\right]\left[\begin{array}{ccc}(b_1-R{a}_1)& ...& (b_N-R{a}_N)\end{array}\right]\left[\begin{array}{c}1\\ ...\\ 1\end{array}\right]& & \\ & =\frac{1}{N}((b_1-R{a}_1{)}^T+...+(b_N-R{a}_N{)}^T)((b_1-R{a}_1)+...+(b_N-R{a}_N))& & \text{矩陣乘法的結合律}\\ & =\frac{1}{N}\sum _{i=1}^N(b_i-R{a}_i{)}^T\sum _{i=1}^N(b_i-R{a}_i)& & \\ & =\frac{1}{N}N({\mu }_b-R{\mu }_a{)}^{T}N({\mu }_b-R{\mu }_a)& & \\ & =N({\mu }_b-R{\mu }_a{)}^T({\mu }_b-R{\mu }_a)& & \\ & =N\Vert {\mu }_b-R{\mu }_a{\Vert }^2& & \end{array}$

將$t=\frac{1}{N}(B-RA)1$代入第三項：
$\begin{array}{rlrl}2\text{tr}((B-RA)1t^T)& =2\text{tr}((B-RA)1\frac{1}{N}{1}^{\mathbf{T}}(B-RA{)}^T)& & \\ & =\frac{2}{N}\text{tr}(\left[\begin{array}{ccc}{b}_1-R{a}_1& ...& b_N-R{a}_N\end{array}\right]\left[\begin{array}{c}1\\ ...\\ 1\end{array}\right]\left[\begin{array}{ccc}1& ...& 1\end{array}\right]\left[\begin{array}{c}(b_1-R{a}_1{)}^T\\ ...\\ (b_N-R{a}_N{)}^T\end{array}\right])& & \\ & =\frac{2}{N}\text{tr}(((b_1-R{a}_1)+...+(b_N-R{a}_N))((b_1-R{a}_1{)}^T+...+(b_N-R{a}_N{)}^T))& & \\ & =\frac{2}{N}\text{tr}((\sum _{i=1}^N{b}_i-R{a}_i)(\sum _{i=1}^N{b}_i-R{a}_i{)}^T)& & \\ & =\frac{2}{N}\text{tr}(N({\mu }_b-R{\mu }_a)N({\mu }_b-R{\mu }_a{)}^T)& & \\ & =2N\text{tr}(({\mu }_b-R{\mu }_a)({\mu }_b-R{\mu }_a{)}^T)& & \text{tr}(c\mathbf{A})=c\text{tr}(\mathbf{A})\\ & =2N\Vert {\mu }_b-R{\mu }_a{\Vert }^2& & \text{tr}(v_1\times v_2)=v_1\cdot v_2\end{array}$

將f轉化為x²+y²-2xy的形式

為了接下來推導方便，將第一項轉化成如下形式：
$\begin{array}{rl}\Vert B-RA{\Vert }_F^2& =\sum _{i=1}^m\sum _{j=1}^N(B-RA{)}_{ij}^2\\ & =\sum _{j=1}^N\sum _{i=1}^m(B-RA{)}_{ij}^2\\ & =\sum _{j=1}^N\Vert b_j-R{a}_j{\Vert }^2\\ & =\sum _{i=1}^N\Vert b_i-R{a}_i{\Vert }^2\end{array}$

第二項由$N\Vert {\mu }_b-R{\mu }_a{\Vert }^2$化為$\sum _{i=1}^N\Vert {\mu }_b-R{\mu }_a{\Vert }^2$。

第三項由$2N\Vert {\mu }_b-R{\mu }_a{\Vert }^2$轉化為：
$\begin{array}{rl}2N\Vert {\mu }_b-R{\mu }_a{\Vert }^2& =2N({\mu }_b-R{\mu }_a{)}^T({\mu }_b-R{\mu }_a)\\ & =2\sum _{i=1}^N(b_i-R{a}_i{)}^T({\mu }_b-R{\mu }_a)\end{array}$

至此，$f(R,t)=\sum _{i=1}^N\Vert b_i-R{a}_i{\Vert }^2+\sum _{i=1}^N\Vert {\mu }_b-R{\mu }_a{\Vert }^2-2\sum _{i=1}^N(b_i-R{a}_i{)}^T({\mu }_b-R{\mu }_a)$

化簡並引入去除均值的點雲

$\begin{array}{rlrl}f(R,t)& =\sum _{i=1}^N\Vert b_i-R{a}_i{\Vert }^2+\sum _{i=1}^N\Vert {\mu }_b-R{\mu }_a{\Vert }^2-2\sum _{i=1}^N(b_i-R{a}_i{)}^T({\mu }_b-R{\mu }_a)& & \\ & =\sum _{i=1}^N(\Vert b_i-R{a}_i{\Vert }^2+\Vert {\mu }_b-R{\mu }_a{\Vert }^2-2(b_i-R{a}_i{)}^T({\mu }_b-R{\mu }_a))& & \\ & =\sum _{i=1}^N(\Vert b_i-R{a}_i{\Vert }^2+\Vert {\mu }_b-R{\mu }_a{\Vert }^2-2\text{tr}((b_i-R{a}_i{)}^T({\mu }_b-R{\mu }_a)))& & (b_i-R{a}_i{)}^T({\mu }_b-R{\mu }_a)\text{是純量}\\ & =\sum _{i=1}^N(\Vert b_i-R{a}_i{\Vert }^2+\Vert {\mu }_b-R{\mu }_a{\Vert }^2-2⟨b_i-R{a}_i,{\mu }_b-R{\mu }_a{⟩}_F)& & \text{Frobenius inner product}\\ & =\sum _{i=1}^N\Vert (b_i-R{a}_i)-({\mu }_b-R{\mu }_a){\Vert }^2& & \text{反向套用Frobenius decomposition}\\ & =\sum _{i=1}^N\Vert (b_i-{\mu }_b)-R(a_i-{\mu }_a){\Vert }^2& & \\ & =\sum _{i=1}^N\Vert b_i^{\prime }-R{a}_i^{\prime }{\Vert }^2& & b_i^{\prime }\triangleq b_i-{\mu }_b,a_i^{\prime }\triangleq a_i-{\mu }_a\\ & =\Vert B^{\prime }-R{A}^{\prime }{\Vert }_F^2& & A^{\prime }\triangleq A(I_N-\frac{1}{N}11^T),B^{\prime }\triangleq B(I_N-\frac{1}{N}11^T)\end{array}$

關於$A^{\prime }$的推導：

$\begin{array}{rlrl}{A}^{\prime }& =A(I_n-\frac{1}{N}11^T)& & \\ & =A-\frac{1}{N}A11^T& & \\ & =\left[\begin{array}{cccc}{A}_{11}& A_{12}& ...& A_{1N}\\ ...& & & \\ A_{m1}& A_{m2}& ...& A_{mN}\end{array}\right]-\frac{1}{N}\left[\begin{array}{cccc}{A}_{11}& A_{12}& ...& A_{1N}\\ ...& & & \\ A_{m1}& A_{m2}& ...& A_{mN}\end{array}\right]\left[\begin{array}{cccc}1& 1& ...& 1\\ 1& 1& ...& 1\\ ...& & & \\ 1& 1& ...& 1\end{array}\right]& & \\ & =\left[\begin{array}{cccc}{A}_{11}& A_{12}& ...& A_{1N}\\ ...& & & \\ A_{m1}& A_{m2}& ...& A_{mN}\end{array}\right]-\frac{1}{N}\left[\begin{array}{cccc}\sum _{j=1}^N{A}_{1j}& \sum _{j=1}^N{A}_{1j}& ...& \sum _{j=1}^N{A}_{1j}\\ ...& & & \\ \sum _{j=1}^N{A}_{mj}& \sum _{j=1}^N{A}_{mj}& ...& \sum _{j=1}^N{A}_{mj}\end{array}\right]& & \\ & =\left[\begin{array}{cccc}{A}_{11}& A_{12}& ...& A_{1N}\\ ...& & & \\ A_{m1}& A_{m2}& ...& A_{mN}\end{array}\right]-\left[\begin{array}{cccc}{\mu }_1& {\mu }_1& ...& {\mu }_1\\ ...& & & \\ {\mu }_m& {\mu }_m& ...& {\mu }_m\end{array}\right]& & {\mu }_i\triangleq \text{A第i個row的平均值}\\ & =\left[\begin{array}{cccc}{A}_{11}-{\mu }_1& A_{12}-{\mu }_1& ...& A_{1N}-{\mu }_1\\ ...& & & \\ A_{m1}-{\mu }_m& A_{m2}-{\mu }_m& ...& A_{mN}-{\mu }_m\end{array}\right]& & \end{array}$

也就是在各個維度（1~m）去除均值的$A$。

SVD矩陣分解

拆開並做SVD矩陣分解：

$\begin{array}{rlrl}f(R,t)& =\Vert B^{\prime }-R{A}^{\prime }{\Vert }_F^2& & \\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert R{A}^{\prime }{\Vert }_F^2-2⟨B^{\prime },R{A}^{\prime }{⟩}_F& & \text{Frobenius decomposition}\\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert R{A}^{\prime }{\Vert }_F^2-2\text{tr}(R{A}^{\prime }{B}^{\prime T})& & ⟨A,B{⟩}_F=\text{tr}(A{B}^T)=\text{tr}(A^{T}B)=\text{tr}(B{A}^T)=\text{tr}(B^{T}A)\\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert A^{\prime }{\Vert }_F^2-2\text{tr}(R{A}^{\prime }{B}^{\prime T})& & \text{旋轉矩陣R不會改變矩陣的norm}\\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert A^{\prime }{\Vert }_F^2-2\text{tr}(RV\Sigma U^T)& & \text{SVD矩陣分解：}{B}^{\prime }{A}^{\prime T}=U\Sigma V^T,A^{\prime }{B}^{\prime T}=V\Sigma U^T\\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert A^{\prime }{\Vert }_F^2-2\text{tr}(U^{T}RV\Sigma )& & \text{tr}(\mathbf{A}\mathbf{B}\mathbf{C}\mathbf{D})=\text{tr}(\mathbf{B}\mathbf{C}\mathbf{D}\mathbf{A})=\text{tr}(\mathbf{C}\mathbf{D}\mathbf{A}\mathbf{B})=\text{tr}(\mathbf{D}\mathbf{A}\mathbf{B}\mathbf{C})\\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert A^{\prime }{\Vert }_F^2-2\text{tr}(T\Sigma )& & \text{在此令}T\triangleq U^{T}RV\\ & =\Vert B^{\prime }{\Vert }_F^2+\Vert A^{\prime }{\Vert }_F^2-2\sum _{i=1}^m{T}_{ii}{\sigma }_i& & \Sigma \text{是}m\times m\text{對角矩陣,}\Sigma =\left[\begin{array}{cccc}{\sigma }_1& 0& ...& 0\\ 0& {\sigma }_2& ...& 0\\ 0& 0& ...& {\sigma }_m\end{array}\right]\end{array}$

尋找最優的R

欲使$f(R,t)=\Vert B^{\prime }{\Vert }_F^2+\Vert A^{\prime }{\Vert }_F^2-2\sum _{i=1}^m{T}_{ii}{\sigma }_i$最小化，等同於求解$\underset{R,t}{\max }(\sum _{i=1}^m{T}_{ii}{\sigma }_i)$。
因為$U$,$R$,$V$皆為正交矩陣，故$T\triangleq U^{T}RV$亦為正交矩陣，所以有$T{T}^T=I_m$且$|T_{ij}|<=1$（參考None element of orthogonal matrix can’t have unit modulus larger then 1）。

在$T=I_m$時$T_{ii}=1$，所以$\sum _{i-1}^m{T}_{ii}{\sigma }_i$有最大值，此時$T\triangleq U^{T}RV=I_m$，這等價於取$R=U{V}^T$。

所以$R=U{V}^T$，$t=\frac{1}{N}(B-RA)1$即為Procrustes Transformation Problem的最優解。