ICP變種Point-To-Plane算法推導

前言

接續ICP（Iterative Closest Point）算法推導，本篇介紹ICP的變種Point-To-Plane損失函數及其求解方法。

本文同樣整理自深藍學院三維點雲處理課程的Lecture 9 – Registration。

損失函數

在一般（Point-To-Point）的ICP中，是以多組點對距離的平方和當作損失函數，在Point-To-Plane中，則是以多組點對經轉換後的source點到target點法向量所定義的平面的距離平方和當作損失函數。

如下圖：

將第$i$組點對表示為$(p_i,q_i)$，其中$q_i$的法向量為$n_i$。

經$R,t$轉換後的$p_i$到$q_i$所在局部擬合平面的距離等於兩者連線$R{p}_i+t-q_i$在$n_i$方向上的投影，也就是$(R{p}_i+t-q_i{)}^T{n}_i$。

考慮所有$1$到$n$組點對，所以有以下損失函數：
$E(R,t)=\sum _{i=1}^n((R{p}_i+t-q_i{)}^T{n}_i{)}^2$

R,t參數化

可以將$R$拆成以$x$為軸、以$y$為軸和以$z$為軸的旋轉，假設在這三個軸上各自旋轉$\alpha ,\beta ,\gamma$度，則有：

$R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\alpha )& -\sin (\alpha )\\ 0& \sin (\alpha )& \cos (\alpha )\end{array}\right]$

$R_y(\beta )=\left[\begin{array}{ccc}\cos (\beta )& 0& \sin (\beta )\\ 0& 1& 0\\ -\sin (\beta )& 0& \cos (\beta )\end{array}\right]$

$R_z(\gamma )=\left[\begin{array}{ccc}\cos (\gamma )& -\sin (\gamma )& 0\\ \sin (\gamma )& \cos (\gamma )& 0\\ 0& 0& 1\end{array}\right]$

旋轉矩陣為它們三者的乘積，先沿$x$軸旋轉，然後依次沿$y,z$軸旋轉：

$R=R_z(\gamma )R_y(\beta )R_x(\alpha )\triangleq \left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$

展開如下：

$\begin{array}{rl}R& =R_z(\gamma )\left[\begin{array}{ccc}\cos (\beta )& 0& \sin (\beta )\\ 0& 1& 0\\ -\sin (\beta )& 0& \cos (\beta )\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\alpha )& -\sin (\alpha )\\ 0& \sin (\alpha )& \cos (\alpha )\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos (\gamma )& -\sin (\gamma )& 0\\ \sin (\gamma )& \cos (\gamma )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos (\beta )& \sin (\alpha )\sin (\beta )& \cos (\alpha )\sin (\beta )\\ 0& \cos (\alpha )& -\sin (\alpha )\\ -\sin (\beta )& \sin (\alpha )\cos (\beta )& \cos (\alpha )\cos (\beta )\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos (\beta )\cos (\gamma )& \sin (\alpha )\sin (\beta )\cos (\gamma )-\cos (\alpha )\sin (\gamma )& \cos (\alpha )\sin (\beta )\cos (\gamma )+\sin (\alpha )\sin (\gamma )\\ \cos (\beta )\sin (\gamma )& \sin (\alpha )\sin (\beta )\sin (\gamma )+\cos (\alpha )\cos (\gamma )& \cos (\alpha )\sin (\beta )\sin (\gamma )-\sin (\alpha )\cos (\gamma )\\ -\sin (\beta )& \sin (\alpha )\cos (\beta )& \cos (\alpha )\cos (\beta )\end{array}\right]\end{array}$

所以$R$為$\alpha ,\beta ,\gamma$的（非線性）函數。

假設每次迭代估計出來的旋轉角度都很小$\alpha ,\beta ,\gamma \to 0$，那麼：
$\cos (\theta )\to 1$，$\sin (\theta )\to \theta$，${\theta }^2\to 0\text{ if }\theta \to 0$。

將上面得到的前兩個近似代入矩陣$R$：

$R\approx \left[\begin{array}{ccc}1\ast 1& \alpha \ast \beta \ast 1-1\ast \gamma & 1\ast \beta \ast 1+\alpha \ast \gamma \\ 1\ast \gamma & \alpha \ast \beta \ast \gamma +1\ast 1& 1\ast \beta \ast \gamma -\alpha \ast 1\\ -\beta & \alpha \ast 1& 1\ast 1\end{array}\right]$

接著套用最後一個近似${\theta }^2\to 0\text{ if }\theta \to 0$，得到：

$R\approx \left[\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}\right]$

至此，$R$已經變為$\alpha ,\beta ,\gamma$的線性函數了。

$t$的參數化很簡單，也是用三個變數來表達：$t=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$。

如此一來損失函數就變成未知數的線性函數了(?)

最小二乘問題

此處的目標是將損失函數表示成$\Vert Ax-b{\Vert }^2$的形式，接下來就可以把它當成最小二乘問題，套用現成的方法求解。

先關注第$i$項的距離（未做平方）：
$\begin{array}{rl}(R{p}_i+t-q_i{)}^T{n}_i& =(\left[\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}\right]\left[\begin{array}{c}{p}_{ix}\\ p_{iy}\\ p_{iz}\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]-\left[\begin{array}{c}{q}_{ix}\\ q_{iy}\\ q_{iz}\end{array}\right]{)}^T{n}_i\\ & ={\left[\begin{array}{c}{p}_{ix}-p_{iy}\gamma +p_{iz}\beta +t_x-q_{ix}\\ p_{ix}\gamma +p_{iy}-p_{iz}\alpha +t_y-q_{iy}\\ -p_{ix}\beta +p_{iy}\alpha +p_{iz}+t_z-q_{iz}\end{array}\right]}^T\left[\begin{array}{c}{n}_{ix}\\ n_{iy}\\ n_{iz}\end{array}\right]\\ & =n_{ix}(p_{ix}-p_{iy}\gamma +p_{iz}\beta +t_x-q_{ix})\\ & +n_{iy}(p_{ix}\gamma +p_{iy}-p_{iz}\alpha +t_y-q_{iy})\\ & +n_{iz}(-p_{ix}\beta +p_{iy}\alpha +p_{iz}+t_z-q_{iz})\\ & =(n_{iz}{p}_{iy}-n_{iy}{p}_{iz})\alpha +(n_{ix}{p}_{iz}-n_{iz}{p}_{ix})\beta +(n_{iy}{p}_{ix}-n_{ix}{p}_{iy})\gamma \\ & +n_{ix}{t}_x+n_{iy}{t}_y+n_{iz}{t}_z\\ & -(n_{ix}{q}_{ix}+n_{iy}{q}_{iy}+n_{iz}{q}_{iz}-n_{ix}{p}_{ix}-n_{iy}{p}_{iy}-n_{iz}{p}_{iz})\end{array}$

注意因為我們希望將損失函數表示為$\Vert Ax-b{\Vert }^2$的形式，所以最後一個符號是減。

定義未知數向量$x\triangleq {\left[\begin{array}{cccccc}\alpha & \beta & \gamma & t_x& t_y& t_z\end{array}\right]}^T$

將所有$i=1～n$的各項納入考慮，定義矩陣$A\triangleq \left[\begin{array}{cccccc}{a}_{11}& a_{12}& a_{13}& n_{1x}& n_{1y}& n_{1z}\\ ...& & & & & \\ a_{i1}& a_{i2}& a_{i3}& n_{ix}& n_{iy}& n_{iz}\\ ...& & & & & \\ a_{N1}& a_{N2}& a_{N3}& n_{Nx}& n_{Ny}& n_{Nz}\end{array}\right]$，

其中$a_{i1}=n_{iz}{p}_{iy}-n_{iy}{p}_{iz}$，$a_{i2}=n_{ix}{p}_{iz}-n_{iz}{p}_{ix}$，$a_{i3}=n_{iy}{p}_{ix}-n_{ix}{p}_{iy}$。

定義矩陣$b\triangleq \left[\begin{array}{c}{b}_1\\ ...\\ b_i\\ ...\\ b_N\end{array}\right]$，

其中$b_i=n_{ix}{q}_{ix}+n_{iy}{q}_{iy}+n_{iz}{q}_{iz}-n_{ix}{p}_{ix}-n_{iy}{p}_{iy}-n_{iz}{p}_{iz}$。

於是我們就可以將損失函數表示為：

$E(R,t)=\sum _{i=1}^n((R{p}_i+t-q_i{)}^T{n}_i{)}^2=\Vert Ax-b{\Vert }^2$

求解

將損失函數表達為最小二乘問題後，接下來就可以用以下方式來求解：

1. Moore–Penrose偽逆
2. SVD矩陣分解

Moore–Penrose偽逆

根據Moore–Penrose inverse， 假設有$A\in {\mathbb{R}}^{m\times n},m>=n$，在$A$的每個column都是線性獨立（column full-rank），即$A^{T}A$是可逆的情況下，有：$A^{+}=(A^{T}A{)}^{-1}{A}^T$，其中$A^{+}A=I$，$A^{+}$稱為$A$的left inverse。

對於$Ax=b$這個方程來說，取$\hat{x}=A^{+}b=(A^{T}A{)}^{-1}{A}^{T}b$可以使得$\Vert Ax-b\Vert$為最小，即最小二乘解。證明可以參考stanford - Least-squares的第二到第四頁。

欲計算$\hat{x}$，可以先分別計算$(A^{T}A{)}^{-1}$和$A^{T}b$，再將它們相乘即可。

首先計算$(A^{T}A{)}^{-1}$：

$\begin{array}{rl}{A}^{T}A& =\left[\begin{array}{ccccc}{a}_{11}& ...& a_{i1}& ...& a_{N1}\\ a_{12}& ...& a_{i2}& ...& a_{N2}\\ a_{13}& ...& a_{i3}& ...& a_{N3}\\ n_{1x}& ...& n_{ix}& ...& n_{Nx}\\ n_{1y}& ...& n_{iy}& ...& n_{Ny}\\ n_{1z}& ...& n_{iz}& ...& n_{Nz}\end{array}\right]\left[\begin{array}{cccccc}{a}_{11}& a_{12}& a_{13}& n_{1x}& n_{1y}& n_{1z}\\ ...& & & & & \\ a_{i1}& a_{i2}& a_{i3}& n_{ix}& n_{iy}& n_{iz}\\ ...& & & & & \\ a_{N1}& a_{N2}& a_{N3}& n_{Nx}& n_{Ny}& n_{Nz}\end{array}\right]\end{array}$

來看看其中的幾項：

$A^{T}A(1,1)=\left[\begin{array}{ccccc}{a}_{11}& ...& a_{i1}& ...& a_{N1}\end{array}\right]\left[\begin{array}{c}{a}_{11}\\ ...\\ a_{i1}\\ ...\\ a_{N1}\end{array}\right]=\sum _{i=1}^N{a}_{i1}^2$

$A^{T}A(1,2)=\left[\begin{array}{ccccc}{a}_{11}& ...& a_{i1}& ...& a_{N1}\end{array}\right]\left[\begin{array}{c}{a}_{12}\\ ...\\ a_{i2}\\ ...\\ a_{N2}\end{array}\right]=\sum _{i=1}^N{a}_{i1}{a}_{i2}$

$A^{T}A(1,4)=\left[\begin{array}{ccccc}{a}_{11}& ...& a_{i1}& ...& a_{N1}\end{array}\right]\left[\begin{array}{c}{n}_{1x}\\ ...\\ n_{ix}\\ ...\\ n_{Nx}\end{array}\right]=\sum _{i=1}^N{a}_{i1}{n}_{ix}$

$A^{T}A(5,6)=\left[\begin{array}{ccccc}{n}_{1y}& ...& n_{iy}& ...& n_{Ny}\end{array}\right]\left[\begin{array}{c}{n}_{1z}\\ ...\\ n_{iz}\\ ...\\ n_{Nz}\end{array}\right]=\sum _{i=1}^N{n}_{iy}{n}_{iz}$

所以：

$A^{T}A=\left[\begin{array}{cccccc}\sum _{i=1}^N{a}_{i1}^2& \sum _{i=1}^N{a}_{i1}{a}_{i2}& \sum _{i=1}^N{a}_{i1}{a}_{i3}& \sum _{i=1}^N{a}_{i1}{n}_{ix}& \sum _{i=1}^N{a}_{i1}{n}_{iy}& \sum _{i=1}^N{a}_{i1}{n}_{iz}\\ \sum _{i=1}^N{a}_{i2}{a}_{i1}& \sum _{i=1}^N{a}_{i2}^2& \sum _{i=1}^N{a}_{i1}{a}_{i3}& \sum _{i=1}^N{a}_{i1}{n}_{ix}& \sum _{i=1}^N{a}_{i1}{n}_{iy}& \sum _{i=1}^N{a}_{i1}{n}_{iz}\\ \sum _{i=1}^N{a}_{i3}{a}_{i1}& \sum _{i=1}^N{a}_{i3}{a}_{i2}& \sum _{i=1}^N{a}_{i3}^2& \sum _{i=1}^N{a}_{i3}{n}_{ix}& \sum _{i=1}^N{a}_{i3}{n}_{iy}& \sum _{i=1}^N{a}_{i3}{n}_{iz}\\ \sum _{i=1}^N{n}_{ix}{a}_{i1}& \sum _{i=1}^N{n}_{ix}{a}_{i2}& \sum _{i=1}^N{n}_{ix}{a}_{i3}& \sum _{i=1}^N{n}_{ix}^2& \sum _{i=1}^N{n}_{ix}{n}_{iy}& \sum _{i=1}^N{n}_{ix}{n}_{iz}\\ \sum _{i=1}^N{n}_{iy}{a}_{i1}& \sum _{i=1}^N{n}_{iy}{a}_{i2}& \sum _{i=1}^N{n}_{iy}{a}_{i3}& \sum _{i=1}^N{n}_{iy}{n}_{ix}& \sum _{i=1}^N{n}_{iy}^2& \sum _{i=1}^N{n}_{iy}{n}_{iz}\\ \sum _{i=1}^N{n}_{iz}{a}_{i1}& \sum _{i=1}^N{n}_{iz}{a}_{i2}& \sum _{i=1}^N{n}_{iz}{a}_{i3}& \sum _{i=1}^N{n}_{iz}{n}_{ix}& \sum _{i=1}^N{n}_{iz}{n}_{iy}& \sum _{i=1}^N{n}_{iz}^2\end{array}\right]$

接著看$A^{T}b$：

$\begin{array}{rl}{A}^{T}b& =\left[\begin{array}{ccccc}{a}_{11}& ...& a_{i1}& ...& a_{N1}\\ a_{12}& ...& a_{i2}& ...& a_{N2}\\ a_{13}& ...& a_{i3}& ...& a_{N3}\\ n_{1x}& ...& n_{ix}& ...& n_{Nx}\\ n_{1y}& ...& n_{iy}& ...& n_{Ny}\\ n_{1z}& ...& n_{iz}& ...& n_{Nz}\end{array}\right]\left[\begin{array}{c}{b}_1\\ ...\\ b_i\\ ...\\ b_N\end{array}\right]\\ & =\left[\begin{array}{c}\sum _{i=1}^N{a}_{i1}{b}_i\\ \sum _{i=1}^N{a}_{i2}{b}_i\\ \sum _{i=1}^N{a}_{i3}{b}_i\\ \sum _{i=1}^N{n}_{ix}{b}_i\\ \sum _{i=1}^N{n}_{iy}{b}_i\\ \sum _{i=1}^N{n}_{iz}{b}_i\end{array}\right]\end{array}$

最後對$(A^{T}A)$取逆，然後左乘$A^{T}b$，即可得到最小二乘解$\hat{x}=(A^{T}A{)}^{-1}{A}^{T}b$

SVD矩陣分解

假設矩陣$A$的奇異值分解為$A=U\Sigma V^{\ast }$，那麼$A^{+}=V{\Sigma }^{+}{U}^{\ast }$。證明參考Pseudoinverse matrix and SVD。