この記事では、KinectFusionのような深度フュージョンパイプラインで使われるICPアルゴリズムについて説明する。

ICPの目的は、2つの点群、すなわち古い点群（3Dモデル内の既存の点と法線）と新しい点群（新しい点と法線、既存のモデルに統合したいもの）を位置合わせすることである。ICPはこれら2つの点群間の回転＋並進変換を返す。

Iterative Closest Point (ICP) は、2つの点群内の対応する点同士の点と平面の距離 (Point to Plane Distance, PPD) である目的関数を最小化する:

$E=\sum_{i}\left\|ppd(p_{i}, q_{i}, n_{i})\right\|_{2}\rightarrow0$

ppd(p, q, n) とは何か?

具体的には、対応する各点 P と Q について、点 P から、点 Q と点 Q に位置する法線 N によって決まる平面までの距離である。2点 P と Q は、現在のカメラ姿勢の下で同じピクセルに投影される場合に対応していると見なされる。

p - 新しい点群内のi番目の点

q - 古い点群内のi番目の点

n - 古い点群内の点 q における法線

したがって、ppd(...) は（p と q の差）と（n）の内積として表せる:

$dot(T_{p2q}(p)-q, n)=dot((R\cdot p + t)-q,n)=[(R\cdot p + t)-q]^{T}\cdot n$

T(p) は点 p の剛体変換である:

$T_{p2q}(p) = (R \cdot p + t)$

ここで R は回転、t は並進である。

T はICPで求める変換であり、その目的は各点 p を点と平面の距離の観点で対応する点 q に近づけることである。

目的関数を最小化するには?

関数の最小化にはガウス・ニュートン法を用いる。

ガウス・ニュートン法では、R と t を関数Eが減少する方向、すなわちその勾配の方向に変化させながら逐次的にステップを進める:

各ステップでは、関数 E をその現在値に、ヤコビ行列に delta x（連結された delta R ベクトルと delta t ベクトル）を掛けたものを加えたものとして線形に近似する。
方程式 E_approx(delta_x) = 0 を解くことで delta R と delta t を求める
delta R と delta t を現在のRt変換に適用し、次の反復に進む

E を線形化するには?

微小な近傍で近似してみよう。

これが R と t を変化させて最小化しようとする式である:

$E=\sum\left\|[(R\cdot p + t)-q]^{T}\cdot n\right\|_{2}$

点と平面の距離は R と t の両方に対して線形だが、回転空間それ自体は線形ではない。これは R がその回転角からどのように生成されるかを見れば分かる:

$\displaystyle R = R_{z}(\gamma)R_{y}(\beta )R_{x}(\alpha)= \begin{bmatrix} cos(\gamma) & -sin(\gamma) & 0 \\ sin(\gamma) & cos(\gamma) & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} cos(\beta) & 0 & sin(\beta)\\ 0 & 1 & 0\\ -sin(\beta) & 0 & cos(\beta) \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & cos(\alpha) & -sin(\alpha)\\ 0 & sin(\alpha) & cos(\alpha) \end{bmatrix}$

しかし、微小回転であるため、R は次の形で近似できる:

$R=I + Ad\theta$

ここで I は単位行列、A は3次元特殊直交群 so(3) の要素である。

すべての sin(t) 項と cos(t) 項を t --> 0 における極限に近づけると、次の表現が得られる:

$R = I + \begin{bmatrix}0 & -\gamma & \beta \\ \gamma & 0 & -\alpha \\ -\beta & \alpha & 0 \end{bmatrix} = I + skew(\begin{bmatrix} \alpha & \beta & \gamma \end{bmatrix}^{T}) = I + skew(R_{shift})$

R の近似を E の式に代入し直すと、次が得られる:

$E_{approx}=\sum\left\|[(I + skew(R_{shift})) \cdot p + t - q]^{T} \cdot n \right \|_{2}$

$E_{approx} = \sum \left \| [I \cdot p + skew(R_{shift}) \cdot p + t - q]^{T} \cdot n \right \|_{2}$

$E_{approx} = \sum \left \| [skew(R_{shift}) \cdot p + t + p- q]^{T} \cdot n \right \|_{2}$

変換のシフトを近似する関数fを導入しよう:

$f(x, p) = skew(R_{shift}) \cdot p + t$

$E_{approx} = \sum \left \| [f(x, p) + p- q]^{T} \cdot n \right \|_{2}$

E_approx を最小化するには?

E_approx は、その微分（すなわち引数の増加による導関数）がゼロのとき最小になる。そこでその微分を求めよう:

$d(E_{approx}) = \sum_i d(\left \| ppd(T_{approx}(p_i), q_i, n_i) \right \|_2) =$ $\sum_i d(ppd(T_{approx}(p_i), q_i, n_i)^2) =$ $\sum_i 2\cdot ppd(...)\cdot d(ppd(T_{approx}(p_i), q_i, n_i))$

ppd を微分しよう:

$d(ppd(T_{approx}(p_i), q_i, n_i)) = d([f(x, p_i) + p_i- q_i]^{T} \cdot n_i) = df(x, p_i)^{T} \cdot n_i = dx^T f'(x, p_i)^T \cdot n_i$

ベクトルxの全変数x_jについて得られるのは次のとおりである:

$\frac{\partial E}{\partial x_{j}} = \sum [2 \cdot (f(x, p) + p - q)^{T} \cdot n] \cdot [f_{j}'(x, p)^{T} \cdot n] = 0$

$\sum [2 \cdot n^{T} \cdot (f(x, p) + p - q)] \cdot [n^{T} \cdot f{}'(x, p)] = 0$

新しい変数を置く: $\triangle p = p - q$

$\sum [2 \cdot n^{T} \cdot (f(x, p) + \triangle p)] \cdot [n^{T} \cdot f{}'(x, p)] = 0$

$\sum [(f(x, p) + \triangle p)^{T} \cdot (n \cdot n^{T})] \cdot f{}'(x, p) = 0$

$\sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [f(x, p) + \triangle p] = 0$

f(x, p) は行列とベクトルの乗算として表せる。それを証明するには、 $cross(a, b) = skew(a) \cdot b = skew(b)^{T} \cdot a$ であることを思い出す必要がある:

$f(x, p) = skew(R_{shift}) \cdot p + t_{shift} = skew(p)^T R_{shift} + t_{shift}$ $f(x, p) = \begin{bmatrix} skew(p)^{T} & I_{3\times 3}\end{bmatrix} \cdot \begin{bmatrix} \triangle R & \triangle t \end{bmatrix}^{T} = G(p) \cdot x$

G(p) は簡略化のために導入する。

$d(f(x, p)) = G(p) \cdot dx = f'(x, p) \cdot dx$ であるため、 $f'(x, p) = G(p)$ が得られる。

$\sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [f(x, p)] = \sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [- \triangle p]$

$\sum G(p)^{T} \cdot [n \cdot n^{T}] \cdot [G(p) \cdot X] = \sum G(p)^{T} \cdot [n \cdot n^{T}] \cdot [- \triangle p]$

新しい値を置く:

$C = G(p)^{T} \cdot n$

$C^{T} = (G(p)^{T} \cdot n)^{T} = n^{T} \cdot G(p)$

置き換えを行う:

$\sum C \cdot C^{T} \cdot X = \sum C \cdot n^{T} \cdot [- \triangle p]$

$\sum C\cdot C^{T}\cdot \begin{bmatrix} \triangle R\\ \triangle t \end{bmatrix} = \sum C \cdot n^{T} \cdot [- \triangle p]$

この方程式を解くことで、各ガウス・ニュートン反復の剛体変換のシフトが得られる。

変換シフトをどのように適用するか?

シフトから回転行列と並進行列を生成し、現在の姿勢行列に得られた行列を掛ける。

シフトの並進部分はそのまま結果の行列に寄与するが、回転部分の生成はもう少し巧妙である。回転シフトは指数化によって so(3) から SO(3) へ変換される。実際、3×1のrshiftベクトルは回転軸に回転角を掛けたものを表す。これからロドリゲス変換を用いて回転行列を得る。詳細については wikiページを参照のこと。