ＩＣＰアルゴリズムメモ - のんびり動機付け

何したい

3次元点の位置合わせをしたい．

アルゴリズム

ICP（Iterative Closest Point）はデータ形状をモデル形状に合わせるアルゴリズム．
点の対応は未知でも良いけど，大まかな位置合わせは行っている前提で，高精度に位置を合わせる．

モデル形状： $A$
データ形状： $B$
収束判定閾： $\tau$

1. 大まかに位置を合わせる．

初期並進： $\textbf{t}_0$
初期回転： $\textbf{R}_0$

2. 近傍点の計算する．
データ点 $\textbf{b}_i$ から最も近いモデル形状 $A$ の点を
${ 　\begin{equation} \boldsymbol{\alpha}_i = \mathop{argmin}\limits_{\textbf{a}_i \in A}||\textbf{a}_i-\textbf{b}_i|| \end{equation} }$
とする．

3. 位置合わせパラメータの計算する．

重心を計算する．

${\displaystyle 　\begin{equation} \boldsymbol{\mu}_{a} = \frac{1}{N}\sum^{N}_{i=1} \boldsymbol{\alpha}_i 　,\qquad \boldsymbol{\mu}_{b} = \frac{1}{N}\sum^{N}_{i=1} \textbf{b}_i \end{equation} }$

共分散行列を計算する．

${\displaystyle \begin{equation} \boldsymbol{\Sigma}_{ab} = \frac{1}{N}\sum^{N}_{i=1} (\boldsymbol{\alpha}_i-\boldsymbol{\mu}_a)(\boldsymbol{b}_i-\boldsymbol{\mu}_b)^{\top} \end{equation} }$

共分散行列を特異値分解する．

${\displaystyle \begin{equation} \boldsymbol{\Sigma}_{ab} = \boldsymbol{U} \begin{pmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{pmatrix} \boldsymbol{V}^{\top} \end{equation} }$

回転行列を計算する．

${\displaystyle \begin{equation} \boldsymbol{R} = \boldsymbol{U} \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\boldsymbol{U}\boldsymbol{V}^{\top}) \end{pmatrix} \boldsymbol{V}^{\top} \end{equation} }$

並進ベクトルを計算する．

${\displaystyle \begin{equation} \boldsymbol{t} = \boldsymbol{\mu}_a - \boldsymbol{R}\boldsymbol{\mu}_b \end{equation} }$

4. 誤差を計算する．
${\displaystyle \begin{equation} d_{err} = \frac{1}{N} \sum^{N}_{i=1} || \boldsymbol{\alpha}_i - \boldsymbol{R}\boldsymbol{b}_i - \boldsymbol{t} ||^{2} \end{equation} }$
　
5. 収束判定をする．